<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN">

<!-- 
	Copyright (C) 2007, 2008, 2009, 2010, 2011. PARP Research Group.
	<http://perception.inf.um.es>
	University of Murcia, Spain.

	This file is part of the QVision library.

	QVision is free software: you can redistribute it and/or modify
	it under the terms of the GNU Lesser General Public License as
	published by the Free Software Foundation, version 3 of the License.

	QVision is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	GNU Lesser General Public License for more details.

	You should have received a copy of the GNU Lesser General Public
	License along with QVision. If not, see <http://www.gnu.org/licenses/>.
-->

<html><head><meta http-equiv="content-Type" content="text/html;charset=UTF-8">
<title>QVision: Qt&#39;s Image, Video and Computer Vision Library</title>
<meta name="title" content="QVision" />
<meta name="dc.title" content="QVision" />
<meta name="url" content="http://perception.inf.um.es/QVision" />
<meta name="author" content="PARP Research Group - http://perception.inf.um.es" />
<meta name="revisit-after" content="30 DAYS"/>
<meta name="robots" content="index,follow"/>
<meta name="classification" content="*">
<meta name="rating" content="Safe For Kids">
<meta name="distribution" content="GLOBAL"/>
<meta name="description" content="Qt's Image, Video and Computer Vision Library"/>
<meta name="page-topic" content="Computer Vision research and prototype programming"/>
<meta name="geo.country" content="ES" />

<!--
Keywords:
By license:		open source, gnu, lgpl, gpl, free
By theme:		computer vision, image processing, robotics, programming, source, development
By usage:		library, toolkit, framework, prototype, application
By programming specs:	object oriented, c++, block programming, reusability, gui, graphical, parallel computing, high performance, GPU, prototyping
Interoperability with:	Qt, GSL, GNU Scientific library, OpenCV, CGAL, QWT, CUDA, mplayer, IPP, Intel Image Performance Primitives, blas, lapack
Functionallity:		image features, matrix algebra, projective geometry, mser, function minimization, function optimization, canny operator, harris operator, corner detection, performance evaluation, cpu usage, graphical interface
Main data-types:	matrix, vector, tensor, quaternion, image, polyline
Video sources:		webcam, camera, stream
Devices:		embedded, desktop computer, laptop, mini-laptop
Authors:		PARP research group. University of Murcia, Spain.
-->

<meta name="keywords" content="augmented reality, sfm, structure from motion, open source, gnu, lgpl, gpl, free, computer vision, image processing, robotics, programming, source, development, library, toolkit, framework, prototype, application, object oriented, c++, block programming, reusability, gui, graphical, parallel computing, high performance, GPU, prototyping, Qt, GSL, GNU Scientific library, OpenCV, CGAL, QWT, CUDA, mplayer, IPP, Intel Image Performance Primitives, blas, lapack, image features, matrix algebra, projective geometry, mser, function minimization, function optimization, canny operator, harris operator, corner detection, performance evaluation, cpu usage, graphical interface, matrix, vector, tensor, quaternion, image, polyline, webcam, camera, stream, embedded, desktop computer, laptop, mini-laptop, University of Murcia, Spain, PARP research group, vision por computador"/>
<meta http-equiv="keywords" content="augmented reality, sfm, structure from motion, open source, gnu, lgpl, gpl, free, computer vision, image processing, robotics, programming, source, development, library, toolkit, framework, prototype, application, object oriented, c++, block programming, reusability, gui, graphical, parallel computing, high performance, GPU, prototyping, Qt, GSL, GNU Scientific library, OpenCV, CGAL, QWT, CUDA, mplayer, IPP, Intel Image Performance Primitives, blas, lapack, image features, matrix algebra, projective geometry, mser, function minimization, function optimization, canny operator, harris operator, corner detection, performance evaluation, cpu usage, graphical interface, matrix, vector, tensor, quaternion, image, polyline, webcam, camera, stream, embedded, desktop computer, laptop, mini-laptop, University of Murcia, Spain, PARP research group, vision por computador"/>
<meta http-equiv="pragma" content="no-cache"/>
<meta http-equiv="title" content="QVision"/>
<link href="doxygen.css" rel="stylesheet" type="text/css" />
<link href="tabs.css" rel="stylesheet" type="text/css" />
<link rel="shortcut icon" href="favicon.ico" />
</head><body>

<table width="100%"><tr>
	<td><a href="http://perception.inf.um.es/"><img src="parp.png" border="0" /> <big>PARP Research Group</big></a></td>
	<td align="right"><a href="http://www.um.es/"><big>Universidad de Murcia</big> <img src="um.png" border="0" /></a></td>
</tr></table>

<hr /><br />

<table width="95%" align="center"><tr><td>

<!-- Generated by Doxygen 1.6.3 -->
<script type="text/javascript"><!--
var searchBox = new SearchBox("searchBox", "search",false,'Search');
--></script>
<div class="navigation" id="top">
  <div class="tabs">
    <ul>
      <li><a href="index.html"><span>Main&nbsp;Page</span></a></li>
      <li><a href="pages.html"><span>Related&nbsp;Pages</span></a></li>
      <li><a href="modules.html"><span>Modules</span></a></li>
      <li><a href="annotated.html"><span>Classes</span></a></li>
      <li class="current"><a href="files.html"><span>Files</span></a></li>
      <li>
        <div id="MSearchBox" class="MSearchBoxInactive">
          <form id="FSearchBox" action="search.php" method="get">
            <img id="MSearchSelect" src="search/search.png" alt=""/>
            <input type="text" id="MSearchField" name="query" value="Search" size="20" accesskey="S" 
                   onfocus="searchBox.OnSearchFieldFocus(true)" 
                   onblur="searchBox.OnSearchFieldFocus(false)"/>
          </form>
        </div>
      </li>
    </ul>
  </div>
  <div class="tabs">
    <ul>
      <li><a href="files.html"><span>File&nbsp;List</span></a></li>
      <li><a href="globals.html"><span>File&nbsp;Members</span></a></li>
    </ul>
  </div>
<h1>src/qvsfm/qvgea/geaoptimization.cpp</h1><a href="geaoptimization_8cpp.html">Go to the documentation of this file.</a><div class="fragment"><pre class="fragment"><a name="l00001"></a>00001 <span class="comment">/*</span>
<a name="l00002"></a>00002 <span class="comment"> *      Copyright (C) 2011, 2012. PARP Research Group.</span>
<a name="l00003"></a>00003 <span class="comment"> *      &lt;http://perception.inf.um.es&gt;</span>
<a name="l00004"></a>00004 <span class="comment"> *      University of Murcia, Spain.</span>
<a name="l00005"></a>00005 <span class="comment"> *</span>
<a name="l00006"></a>00006 <span class="comment"> *      This file is part of the QVision library.</span>
<a name="l00007"></a>00007 <span class="comment"> *</span>
<a name="l00008"></a>00008 <span class="comment"> *      QVision is free software: you can redistribute it and/or modify</span>
<a name="l00009"></a>00009 <span class="comment"> *      it under the terms of the GNU Lesser General Public License as</span>
<a name="l00010"></a>00010 <span class="comment"> *      published by the Free Software Foundation, version 3 of the License.</span>
<a name="l00011"></a>00011 <span class="comment"> *</span>
<a name="l00012"></a>00012 <span class="comment"> *      QVision is distributed in the hope that it will be useful,</span>
<a name="l00013"></a>00013 <span class="comment"> *      but WITHOUT ANY WARRANTY; without even the implied warranty of</span>
<a name="l00014"></a>00014 <span class="comment"> *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the</span>
<a name="l00015"></a>00015 <span class="comment"> *      GNU Lesser General Public License for more details.</span>
<a name="l00016"></a>00016 <span class="comment"> *</span>
<a name="l00017"></a>00017 <span class="comment"> *      You should have received a copy of the GNU Lesser General Public</span>
<a name="l00018"></a>00018 <span class="comment"> *      License along with QVision. If not, see &lt;http://www.gnu.org/licenses/&gt;.</span>
<a name="l00019"></a>00019 <span class="comment"> */</span>
<a name="l00020"></a>00020 
<a name="l00024"></a>00024 
<a name="l00025"></a>00025 <span class="preprocessor">#include &lt;QTime&gt;</span>
<a name="l00026"></a>00026 <span class="preprocessor">#include &lt;<a class="code" href="geaoptimization_8h.html" title="File from the QVision library.">qvsfm/qvgea/geaoptimization.h</a>&gt;</span>
<a name="l00027"></a>00027 <span class="preprocessor">#include &lt;QVSparseBlockMatrix&gt;</span>
<a name="l00028"></a>00028 <span class="preprocessor">#include &lt;<a class="code" href="so3EssentialEvaluation_8h.html" title="File from the QVision library.">qvsfm/qvgea/so3EssentialEvaluation.h</a>&gt;</span>
<a name="l00029"></a>00029 <span class="preprocessor">#include &lt;<a class="code" href="quaternionEssentialEvaluation_8h.html" title="File from the QVision library.">qvsfm/qvgea/quaternionEssentialEvaluation.h</a>&gt;</span>
<a name="l00030"></a>00030 
<a name="l00031"></a>00031 QList&lt; QHash&lt; int, QPointF &gt; &gt; storePointMatchingsInTrackingContainer(<span class="keyword">const</span> <a class="code" href="classQVDirectedGraph.html" title="Directed graph class.">QVDirectedGraph</a>&lt; QVector&lt;QPointFMatching&gt; &gt; &amp;matchingLists)
<a name="l00032"></a>00032         {
<a name="l00033"></a>00033         QList&lt; QHash&lt; int, QPointF &gt; &gt; result;
<a name="l00034"></a>00034 
<a name="l00035"></a>00035         <span class="keywordflow">foreach</span>(<a class="code" href="qvdirectedgraph_8h.html#a75d6b5c47d6dbccd956af9be24692e46" title="Link between two nodes in a graph.">QVGraphLink</a> link, matchingLists.keys())
<a name="l00036"></a>00036                 {
<a name="l00037"></a>00037                 <span class="keyword">const</span> QVector&lt;QPointFMatching&gt; &amp;matchings = matchingLists[link];
<a name="l00038"></a>00038                 <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i = 0; i &lt; matchings.count(); i++)
<a name="l00039"></a>00039                         {
<a name="l00040"></a>00040                         QHash&lt; int, QPointF &gt; tracking;
<a name="l00041"></a>00041                         tracking[link.x()] = matchings[i].first;
<a name="l00042"></a>00042                         tracking[link.y()] = matchings[i].second;
<a name="l00043"></a>00043                         result &lt;&lt; tracking;
<a name="l00044"></a>00044                         }
<a name="l00045"></a>00045                 }
<a name="l00046"></a>00046         <span class="keywordflow">return</span> result;
<a name="l00047"></a>00047         }
<a name="l00048"></a>00048 
<a name="l00049"></a><a class="code" href="group__qvsfm.html#gae214f6f61331a85f59b5b9ed655332c3">00049</a> <a class="code" href="classQVDirectedGraph.html" title="Directed graph class.">QVDirectedGraph&lt; QList&lt;QPointFMatching&gt;</a> &gt; <a class="code" href="group__qvsfm.html#gae214f6f61331a85f59b5b9ed655332c3" title="Obtains the list of point correspondences detected between each view-pair in a reconstruction...">getPointMatchingsLists</a>(<span class="keyword">const</span> QList&lt;QHash&lt;int, QPointF&gt; &gt; pointProjections, <span class="keyword">const</span> <span class="keywordtype">int</span> numCams, <span class="keyword">const</span> <span class="keywordtype">int</span> minPointCorrespondences)
<a name="l00050"></a>00050     {
<a name="l00051"></a>00051     QTime time;
<a name="l00052"></a>00052     QHash&lt;int, QPointF&gt; projections;
<a name="l00053"></a>00053 
<a name="l00054"></a>00054     <span class="comment">// Transversing the structure &#39;pointProjections&#39; is not computationally expensive.</span>
<a name="l00055"></a>00055     <span class="comment">// Precompute number of correspondences between each view pair.</span>
<a name="l00056"></a>00056     time.start();
<a name="l00057"></a>00057     QVector&lt; int &gt; count(numCams*numCams, 0);
<a name="l00058"></a>00058     <span class="comment">//if (minPointCorrespondences &gt; 0)</span>
<a name="l00059"></a>00059     <span class="keywordflow">foreach</span>(projections, pointProjections)                      <span class="comment">// 3D points</span>
<a name="l00060"></a>00060         {
<a name="l00061"></a>00061         <span class="keyword">const</span> QVector&lt;int&gt; indexes = projections.keys().toVector();
<a name="l00062"></a>00062         <span class="keywordflow">foreach</span>(<span class="keywordtype">int</span> i, indexes)
<a name="l00063"></a>00063             <span class="keywordflow">foreach</span>(<span class="keywordtype">int</span> j, indexes)
<a name="l00064"></a>00064                 count[i * numCams + j]++;
<a name="l00065"></a>00065         }
<a name="l00066"></a>00066     <span class="comment">// int time1 = time.elapsed();</span>
<a name="l00067"></a>00067 
<a name="l00068"></a>00068     <span class="comment">// ---------------------------------------------------</span>
<a name="l00069"></a>00069 
<a name="l00070"></a>00070     <span class="comment">// Gets the lists of point matchings.</span>
<a name="l00071"></a>00071     <span class="comment">// Main performance bottleneck of function &#39;getPointMatchingsLists&#39;.</span>
<a name="l00072"></a>00072     <span class="comment">//time.start();</span>
<a name="l00073"></a>00073     QVector&lt; QList&lt;QPointFMatching&gt; &gt; v(numCams*numCams);
<a name="l00074"></a>00074     <span class="keywordflow">foreach</span>(projections, pointProjections)
<a name="l00075"></a>00075         {
<a name="l00076"></a>00076         <span class="keyword">const</span> QVector&lt;int&gt; indexes = projections.keys().toVector();
<a name="l00077"></a>00077 
<a name="l00078"></a>00078         <span class="comment">// The most computationally costly part in this process is copying QPoint objects</span>
<a name="l00079"></a>00079         <span class="comment">// to QPointFMatching data structures, and storing them at their corresponding lists.</span>
<a name="l00080"></a>00080         <span class="keywordflow">foreach</span>(<span class="keywordtype">int</span> i, indexes)
<a name="l00081"></a>00081             <span class="keywordflow">foreach</span>(<span class="keywordtype">int</span> j, indexes)
<a name="l00082"></a>00082                 <span class="keywordflow">if</span> ( (i &lt; j) and (count[i*numCams+j] &gt;= minPointCorrespondences) )
<a name="l00083"></a>00083                     v[i * numCams + j] &lt;&lt; <a class="code" href="group__qvmath.html#ga818fe0c689af956cd14431148c879718">QPointFMatching</a>(projections[i], projections[j]);
<a name="l00084"></a>00084         }
<a name="l00085"></a>00085     <span class="comment">//int time5 = time.elapsed();</span>
<a name="l00086"></a>00086 
<a name="l00087"></a>00087     <span class="comment">// Gets the lists of point matchings.</span>
<a name="l00088"></a>00088     <span class="comment">// Main performance bottleneck of function &#39;getPointMatchingsLists&#39;.</span>
<a name="l00089"></a>00089     <span class="comment">/*time.start();</span>
<a name="l00090"></a>00090 <span class="comment">    QList&lt;QPointFMatching&gt; v2;</span>
<a name="l00091"></a>00091 <span class="comment">    //QVector&lt; QList&lt;QPoint&gt; &gt; v2(numCams*numCams);</span>
<a name="l00092"></a>00092 <span class="comment">    foreach(projections, pointProjections)</span>
<a name="l00093"></a>00093 <span class="comment">        {</span>
<a name="l00094"></a>00094 <span class="comment">        const QVector&lt;int&gt; indexes = projections.keys().toVector();</span>
<a name="l00095"></a>00095 <span class="comment"></span>
<a name="l00096"></a>00096 <span class="comment">        // The most computationally costly part in this process is copying QPoint objects</span>
<a name="l00097"></a>00097 <span class="comment">        // to QPointFMatching data structures, and storing them at their corresponding lists.</span>
<a name="l00098"></a>00098 <span class="comment">        foreach(int i, indexes)</span>
<a name="l00099"></a>00099 <span class="comment">            foreach(int j, indexes)</span>
<a name="l00100"></a>00100 <span class="comment">                if ( (i &lt; j) and (count[i*numCams+j] &gt;= minPointCorrespondences) )</span>
<a name="l00101"></a>00101 <span class="comment">                    v2 &lt;&lt; QPointFMatching(projections[i], projections[j]);</span>
<a name="l00102"></a>00102 <span class="comment">        }</span>
<a name="l00103"></a>00103 <span class="comment">    int time6 = time.elapsed();*/</span>
<a name="l00104"></a>00104 
<a name="l00105"></a>00105     <span class="comment">// Then, compose the directed graph with them.</span>
<a name="l00106"></a>00106     <span class="comment">//time.start();</span>
<a name="l00107"></a>00107     <a class="code" href="classQVDirectedGraph.html" title="Directed graph class.">QVDirectedGraph&lt; QList&lt;QPointFMatching&gt;</a> &gt; result;
<a name="l00108"></a>00108     <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i = 0; i &lt; numCams; i++)
<a name="l00109"></a>00109         <span class="keywordflow">for</span>(<span class="keywordtype">int</span> j = 0; j &lt; numCams; j++)
<a name="l00110"></a>00110             {
<a name="l00111"></a>00111             <span class="keyword">const</span> QList&lt;QPointFMatching&gt; &amp; list = v[i*numCams+j];
<a name="l00112"></a>00112             <span class="keywordflow">if</span> (list.count() &gt; 0)
<a name="l00113"></a>00113                 result[ <a class="code" href="qvdirectedgraph_8h.html#a75d6b5c47d6dbccd956af9be24692e46" title="Link between two nodes in a graph.">QVGraphLink</a>(i,j) ] = list;
<a name="l00114"></a>00114             }
<a name="l00115"></a>00115     <span class="comment">//int time6 = time.elapsed();</span>
<a name="l00116"></a>00116 
<a name="l00117"></a>00117     <span class="comment">//std::cout &lt;&lt; &quot;*** time1 = &quot; &lt;&lt; time1 &lt;&lt; std::endl;</span>
<a name="l00118"></a>00118     <span class="comment">//std::cout &lt;&lt; &quot;*** time2 = &quot; &lt;&lt; time2 &lt;&lt; std::endl;</span>
<a name="l00119"></a>00119     <span class="comment">//std::cout &lt;&lt; &quot;*** time3 = &quot; &lt;&lt; time3 &lt;&lt; std::endl;</span>
<a name="l00120"></a>00120     <span class="comment">//std::cout &lt;&lt; &quot;*** time4 = &quot; &lt;&lt; time4 &lt;&lt; std::endl;</span>
<a name="l00121"></a>00121     <span class="comment">//std::cout &lt;&lt; &quot;*** time5 = &quot; &lt;&lt; time5 &lt;&lt; std::endl;</span>
<a name="l00122"></a>00122     <span class="comment">//std::cout &lt;&lt; &quot;*** time6 = &quot; &lt;&lt; time6 &lt;&lt; std::endl;</span>
<a name="l00123"></a>00123     <span class="comment">//std::cout &lt;&lt; &quot;*** time7 = &quot; &lt;&lt; time7 &lt;&lt; std::endl;</span>
<a name="l00124"></a>00124     <span class="keywordflow">return</span> result;
<a name="l00125"></a>00125     }
<a name="l00126"></a>00126 
<a name="l00127"></a>00127 <a class="code" href="classQVDirectedGraph.html" title="Directed graph class.">QVDirectedGraph&lt; QList&lt;QPointFMatching&gt;</a> &gt; pointMatchingsListOld(<span class="keyword">const</span> QList&lt;QHash&lt;int, QPointF&gt; &gt; pointProjections, <span class="keyword">const</span> <span class="keywordtype">int</span> numCams)
<a name="l00128"></a>00128     {
<a name="l00129"></a>00129     Q_UNUSED(numCams);
<a name="l00130"></a>00130 
<a name="l00131"></a>00131     <a class="code" href="classQVDirectedGraph.html" title="Directed graph class.">QVDirectedGraph&lt; QList&lt;QPointFMatching&gt;</a> &gt; result;
<a name="l00132"></a>00132 
<a name="l00133"></a>00133     QHash&lt;int, QPointF&gt; projections;
<a name="l00134"></a>00134     <span class="keywordflow">foreach</span>(projections, pointProjections)                      <span class="comment">// 3D points</span>
<a name="l00135"></a>00135         <span class="keywordflow">foreach</span>(<span class="keywordtype">int</span> i, projections.keys())              <span class="comment">// views</span>
<a name="l00136"></a>00136             <span class="keywordflow">foreach</span>(<span class="keywordtype">int</span> j, projections.keys())
<a name="l00137"></a>00137                 <span class="keywordflow">if</span> (i &lt; j)
<a name="l00138"></a>00138                     result[ <a class="code" href="qvdirectedgraph_8h.html#a75d6b5c47d6dbccd956af9be24692e46" title="Link between two nodes in a graph.">QVGraphLink</a>(i,j) ] &lt;&lt; <a class="code" href="group__qvmath.html#ga818fe0c689af956cd14431148c879718">QPointFMatching</a>(projections[i], projections[j]);
<a name="l00139"></a>00139     <span class="keywordflow">return</span> result;
<a name="l00140"></a>00140     }
<a name="l00141"></a>00141 
<a name="l00142"></a><a class="code" href="group__qvsfm.html#ga528d393cab737319ae4b2d6447c5cccc">00142</a> <a class="code" href="classQVDirectedGraph.html" title="Directed graph class.">QVDirectedGraph&lt;QVMatrix&gt;</a>  <a class="code" href="group__qvsfm.html#ga528d393cab737319ae4b2d6447c5cccc" title="Gets the reduced matrices for a reconstruction.">getReducedMatrices</a>(  <span class="keyword">const</span> <a class="code" href="classQVDirectedGraph.html" title="Directed graph class.">QVDirectedGraph</a>&lt; QList&lt;QPointFMatching&gt; &gt; &amp;pointLists,
<a name="l00143"></a>00143                             <span class="keyword">const</span> <span class="keywordtype">bool</span> normalize,
<a name="l00144"></a>00144                             <span class="keyword">const</span> TGEA_decomposition_method decomposition_method,
<a name="l00145"></a>00145                             <span class="keyword">const</span> <span class="keywordtype">bool</span> gsl,
<a name="l00146"></a>00146                             <span class="keyword">const</span> <span class="keywordtype">double</span> choleskyLambda,
<a name="l00147"></a>00147                             <span class="keyword">const</span> <span class="keywordtype">int</span> minPointCorrespondences)
<a name="l00148"></a>00148     {
<a name="l00149"></a>00149     <span class="comment">//std::cout &lt;&lt; &quot;WARNING: &#39;getReducedMatrices&#39; deprecated. Use functions &#39;getSquaredDLTMatrix&#39; and &#39;getDLTMatrix&#39; to obtain the reduced matrices, and &#39;globalEpipolarAdjustment2&#39; to perform GEA optimization.&quot; &lt;&lt; std::endl;</span>
<a name="l00150"></a>00150     <a class="code" href="classQVDirectedGraph.html" title="Directed graph class.">QVDirectedGraph&lt;QVMatrix&gt;</a> reducedMatricesGraph;
<a name="l00151"></a>00151 
<a name="l00152"></a>00152     <span class="comment">// For each pair of views linked with point correspondences...</span>
<a name="l00153"></a>00153     <span class="keywordflow">foreach</span>(QPoint p, pointLists.keys())
<a name="l00154"></a>00154         {
<a name="l00155"></a>00155         <span class="keyword">const</span> QList&lt;QPointFMatching&gt; &amp;pointList = pointLists[p];
<a name="l00156"></a>00156         <span class="keywordflow">if</span> (pointList.count() &lt; minPointCorrespondences)
<a name="l00157"></a>00157             <span class="keywordflow">continue</span>;
<a name="l00158"></a>00158 
<a name="l00159"></a>00159         <span class="comment">// Evaluate reduced coefficient matrix.</span>
<a name="l00160"></a>00160         reducedMatricesGraph[p] = getReduced8PointsCoefficientsMatrix(pointList, decomposition_method, normalize, gsl, choleskyLambda);
<a name="l00161"></a>00161         }
<a name="l00162"></a>00162 
<a name="l00163"></a>00163     <span class="keywordflow">return</span> reducedMatricesGraph;
<a name="l00164"></a>00164     }
<a name="l00165"></a>00165 
<a name="l00166"></a><a class="code" href="group__qvsfm.html#ga3a9458f96d471267a14514a22642507e">00166</a> <a class="code" href="classQVDirectedGraph.html" title="Directed graph class.">QVDirectedGraph&lt;QVMatrix&gt;</a>  <a class="code" href="group__qvsfm.html#ga528d393cab737319ae4b2d6447c5cccc" title="Gets the reduced matrices for a reconstruction.">getReducedMatrices</a>(  <span class="keyword">const</span> <a class="code" href="classQVDirectedGraph.html" title="Directed graph class.">QVDirectedGraph</a>&lt; QVector&lt;QPointFMatching&gt; &gt; &amp;pointLists,
<a name="l00167"></a>00167                         <span class="keyword">const</span> <span class="keywordtype">bool</span> normalize,
<a name="l00168"></a>00168                         <span class="keyword">const</span> TGEA_decomposition_method decomposition_method,
<a name="l00169"></a>00169                         <span class="keyword">const</span> <span class="keywordtype">bool</span> gsl,
<a name="l00170"></a>00170                         <span class="keyword">const</span> <span class="keywordtype">double</span> choleskyLambda,
<a name="l00171"></a>00171                         <span class="keyword">const</span> <span class="keywordtype">int</span> minPointCorrespondences)
<a name="l00172"></a>00172     {
<a name="l00173"></a>00173     <span class="comment">//std::cout &lt;&lt; &quot;WARNING: &#39;getReducedMatrices&#39; deprecated. Use functions &#39;getSquaredDLTMatrix&#39; and &#39;getDLTMatrix&#39; to obtain the reduced matrices, and &#39;globalEpipolarAdjustment2&#39; to perform GEA optimization.&quot; &lt;&lt; std::endl;</span>
<a name="l00174"></a>00174     <a class="code" href="classQVDirectedGraph.html" title="Directed graph class.">QVDirectedGraph&lt;QVMatrix&gt;</a> reducedMatricesGraph;
<a name="l00175"></a>00175 
<a name="l00176"></a>00176     <span class="comment">// For each pair of views linked with point correspondences...</span>
<a name="l00177"></a>00177     <span class="keywordflow">foreach</span>(QPoint p, pointLists.keys())
<a name="l00178"></a>00178         {
<a name="l00179"></a>00179         <span class="keyword">const</span> QVector&lt;QPointFMatching&gt; &amp;pointList = pointLists[p];
<a name="l00180"></a>00180         <span class="keywordflow">if</span> (pointList.count() &lt; minPointCorrespondences)
<a name="l00181"></a>00181             <span class="keywordflow">continue</span>;
<a name="l00182"></a>00182 
<a name="l00183"></a>00183         <span class="comment">// Evaluate reduced coefficient matrix.</span>
<a name="l00184"></a>00184         reducedMatricesGraph[p] = getReduced8PointsCoefficientsMatrix(pointList.toList(), decomposition_method, normalize, gsl, choleskyLambda);
<a name="l00185"></a>00185         }
<a name="l00186"></a>00186 
<a name="l00187"></a>00187     <span class="keywordflow">return</span> reducedMatricesGraph;
<a name="l00188"></a>00188     }
<a name="l00189"></a>00189 
<a name="l00190"></a>00190 
<a name="l00191"></a>00191 <span class="comment">// -------------------------------------------------------------------------</span>
<a name="l00192"></a>00192 
<a name="l00193"></a>00193 <span class="comment">// Returns M = DLT matrix</span>
<a name="l00194"></a>00194 <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> get8PointsCoefficientMatrix(<span class="keyword">const</span> QVector&lt;QPointFMatching&gt; &amp;matchings, <span class="keyword">const</span> <span class="keywordtype">bool</span> normalize)
<a name="l00195"></a>00195         {
<a name="l00196"></a>00196         <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> A(matchings.size(),9);
<a name="l00197"></a>00197         <span class="keywordtype">double</span> *aptr = A.<a class="code" href="classQVMatrix.html#a0c5b1e685d14d230c55cfcfb058e372a" title="Gets a reference of the data buffer of the matrix for read and write accesses.">getWriteData</a>();
<a name="l00198"></a>00198 
<a name="l00199"></a>00199         <span class="keywordflow">foreach</span>(<a class="code" href="group__qvmath.html#ga818fe0c689af956cd14431148c879718">QPointFMatching</a> matching,matchings)
<a name="l00200"></a>00200                 {
<a name="l00201"></a>00201                 <span class="comment">//const QPointF sourcePoint = matchings[i].first, destPoint = matchings[i].second;</span>
<a name="l00202"></a>00202                 <span class="comment">//const QPointF sourcePoint = matching.first, destPoint = matching.second;</span>
<a name="l00203"></a>00203                 <span class="comment">//double        x = sourcePoint.x(), y = sourcePoint.y(),</span>
<a name="l00204"></a>00204         <span class="comment">//      x_p = destPoint.x(), y_p = destPoint.y(),</span>
<a name="l00205"></a>00205         <span class="comment">//      normalizer = 1.0/sqrt((x*x + y*y +1)*(x_p*x_p+y_p*y_p+1)),</span>
<a name="l00206"></a>00206         <span class="comment">//      x_ = x * normalizer, y_ = y * normalizer;</span>
<a name="l00207"></a>00207 
<a name="l00208"></a>00208                 <span class="keyword">const</span> QPointF sourcePoint = matching.first, destPoint = matching.second;
<a name="l00209"></a>00209         <span class="keyword">const</span> <span class="keywordtype">double</span>    x = sourcePoint.x(), y = sourcePoint.y(),
<a name="l00210"></a>00210                 x_p = destPoint.x(), y_p = destPoint.y(),
<a name="l00211"></a>00211                 normalizer = normalize?1.0/sqrt((x*x + y*y +1)*(x_p*x_p+y_p*y_p+1)):1.0,
<a name="l00212"></a>00212                 x_ = x * normalizer, y_ = y * normalizer;
<a name="l00213"></a>00213                 <span class="comment">// Faster:</span>
<a name="l00214"></a>00214                 *aptr = x_*x_p;         aptr++;
<a name="l00215"></a>00215                 *aptr = y_*x_p;         aptr++;
<a name="l00216"></a>00216                 *aptr = normalizer*x_p; aptr++;
<a name="l00217"></a>00217                 *aptr = x_*y_p;         aptr++;
<a name="l00218"></a>00218                 *aptr = y_*y_p;         aptr++;
<a name="l00219"></a>00219                 *aptr = normalizer*y_p; aptr++;
<a name="l00220"></a>00220                 *aptr = x_;             aptr++;
<a name="l00221"></a>00221                 *aptr = y_;             aptr++;
<a name="l00222"></a>00222                 *aptr = normalizer;     aptr++;
<a name="l00223"></a>00223                 }
<a name="l00224"></a>00224         <span class="keywordflow">return</span> A;
<a name="l00225"></a>00225         }
<a name="l00226"></a>00226 
<a name="l00227"></a>00227 <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> getTransposeProductOf8PointsCoefficientMatrix(<span class="keyword">const</span> QVector&lt;QPointFMatching&gt; &amp;matchings, <span class="keyword">const</span> <span class="keywordtype">bool</span> normalize)
<a name="l00228"></a>00228         {
<a name="l00229"></a>00229     <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> A(9,9, 0.0);
<a name="l00230"></a>00230     <span class="keywordtype">double</span> *ptrA = A.getWriteData();
<a name="l00231"></a>00231 
<a name="l00232"></a>00232         <span class="keywordflow">foreach</span>(<a class="code" href="group__qvmath.html#ga818fe0c689af956cd14431148c879718">QPointFMatching</a> matching, matchings)
<a name="l00233"></a>00233                 {
<a name="l00234"></a>00234         <span class="keywordtype">double</span> v[9];
<a name="l00235"></a>00235 
<a name="l00236"></a>00236                 <span class="keyword">const</span> QPointF sourcePoint = matching.first, destPoint = matching.second;
<a name="l00237"></a>00237         <span class="keyword">const</span> <span class="keywordtype">double</span>    x = sourcePoint.x(), y = sourcePoint.y(),
<a name="l00238"></a>00238                 x_p = destPoint.x(), y_p = destPoint.y(),
<a name="l00239"></a>00239                 normalizer = normalize?1.0/sqrt((x*x + y*y +1)*(x_p*x_p+y_p*y_p+1)):1.0,
<a name="l00240"></a>00240                 x_ = x * normalizer, y_ = y * normalizer;
<a name="l00241"></a>00241 
<a name="l00242"></a>00242                 <span class="comment">// Faster:</span>
<a name="l00243"></a>00243         v[0] = x_*x_p;
<a name="l00244"></a>00244                 v[1] = y_*x_p;
<a name="l00245"></a>00245                 v[2] = normalizer * x_p;
<a name="l00246"></a>00246                 v[3] = x_*y_p;
<a name="l00247"></a>00247                 v[4] = y_*y_p;
<a name="l00248"></a>00248                 v[5] = normalizer * y_p;
<a name="l00249"></a>00249                 v[6] = x_;
<a name="l00250"></a>00250                 v[7] = y_;
<a name="l00251"></a>00251                 v[8] = normalizer;
<a name="l00252"></a>00252 
<a name="l00253"></a>00253         <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i = 0; i &lt; 9; i++)
<a name="l00254"></a>00254             <span class="keywordflow">for</span>(<span class="keywordtype">int</span> j = 0; j &lt;= i; j++)
<a name="l00255"></a>00255                 ptrA[9*i+j] += v[i] * v[j];
<a name="l00256"></a>00256                 }
<a name="l00257"></a>00257 
<a name="l00258"></a>00258     <span class="comment">// Only store lower diagonal values</span>
<a name="l00259"></a>00259     <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i = 0; i &lt; 9; i++)
<a name="l00260"></a>00260         <span class="keywordflow">for</span>(<span class="keywordtype">int</span> j = i+1; j &lt; 9; j++)
<a name="l00261"></a>00261                 ptrA[9*i+j] = ptrA[9*j+i];
<a name="l00262"></a>00262 
<a name="l00263"></a>00263         <span class="keywordflow">return</span> A;
<a name="l00264"></a>00264         }
<a name="l00265"></a>00265 
<a name="l00266"></a>00266 
<a name="l00267"></a>00267 <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> getReduced8PointsCoefficientsMatrix(   <span class="keyword">const</span> QVector&lt;QPointFMatching&gt; &amp;matchingsList,
<a name="l00268"></a>00268                         <span class="keyword">const</span> TGEA_decomposition_method decomposition_method,
<a name="l00269"></a>00269                         <span class="keyword">const</span> <span class="keywordtype">bool</span> normalize,
<a name="l00270"></a>00270                         <span class="keyword">const</span> <span class="keywordtype">bool</span> use_gsl,
<a name="l00271"></a>00271                         <span class="keyword">const</span> <span class="keywordtype">double</span> choleskyLambda)
<a name="l00272"></a>00272     {
<a name="l00273"></a>00273     <span class="keywordflow">if</span> (matchingsList.count() &gt; 9)
<a name="l00274"></a>00274         {
<a name="l00275"></a>00275         <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> MtM = getTransposeProductOf8PointsCoefficientMatrix(matchingsList, normalize);
<a name="l00276"></a>00276 
<a name="l00277"></a>00277         <span class="keywordflow">switch</span>(decomposition_method)
<a name="l00278"></a>00278             {
<a name="l00279"></a>00279             <span class="keywordflow">case</span> GEA_DO_NOT_DECOMPOSE:
<a name="l00280"></a>00280                 <span class="keywordflow">return</span> MtM;
<a name="l00281"></a>00281                 <span class="keywordflow">break</span>;
<a name="l00282"></a>00282 
<a name="l00283"></a>00283             <span class="keywordflow">case</span> GEA_CHOLESKY_DECOMPOSITION:
<a name="l00284"></a>00284                 {
<a name="l00285"></a>00285                 <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> L;
<a name="l00286"></a>00286 
<a name="l00287"></a>00287                 <span class="comment">// Add lambda to MtM diagonal values to avoid</span>
<a name="l00288"></a>00288                 <span class="comment">// non-positive definite matrix errors.</span>
<a name="l00289"></a>00289                 <span class="keywordflow">if</span> (choleskyLambda != 0.0)
<a name="l00290"></a>00290                     {
<a name="l00291"></a>00291                     <span class="keyword">const</span> <span class="keywordtype">int</span> n = MtM.<a class="code" href="classQVMatrix.html#a420bba03aeccbd18161418049a025f66" title="Get width of the matrix.">getCols</a>();
<a name="l00292"></a>00292                     <span class="keywordtype">double</span> *data = MtM.<a class="code" href="classQVMatrix.html#a0c5b1e685d14d230c55cfcfb058e372a" title="Gets a reference of the data buffer of the matrix for read and write accesses.">getWriteData</a>();
<a name="l00293"></a>00293                     <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i = 0, idx = 0; i &lt; n; i++, idx += n+1)
<a name="l00294"></a>00294                         data[idx] += choleskyLambda;
<a name="l00295"></a>00295                     }
<a name="l00296"></a>00296 
<a name="l00297"></a>00297                 <a class="code" href="group__qvmatrixalgebra.html#gab845aa50ce874199ad7676904829b932" title="Obtains the Cholesky decomposition of a symmetric and positive definite matrix.">CholeskyDecomposition</a>(MtM, L, use_gsl?<a class="code" href="group__qvmatrixalgebra.html#ggac52aa1a4772c42c68e53baf5a22e15c8ad805bb0106a0bbf7eea0549f4a5525d1">GSL_CHOLESKY</a>:<a class="code" href="group__qvmatrixalgebra.html#ggac52aa1a4772c42c68e53baf5a22e15c8a80e13036cfa7096deb292e1b197459bf">LAPACK_CHOLESKY_DPOTRF</a>);
<a name="l00298"></a>00298 
<a name="l00299"></a>00299                 <span class="keywordflow">return</span> L;
<a name="l00300"></a>00300                 }
<a name="l00301"></a>00301                 <span class="keywordflow">break</span>;
<a name="l00302"></a>00302 
<a name="l00303"></a>00303             <span class="keywordflow">case</span> GEA_EIGEN_DECOMPOSITION:
<a name="l00304"></a>00304                 {
<a name="l00305"></a>00305                 <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> Q;
<a name="l00306"></a>00306                 <a class="code" href="classQVVector.html" title="Implementation of numerical vectors.">QVVector</a> lambda;
<a name="l00307"></a>00307 
<a name="l00308"></a>00308                 <a class="code" href="group__qvmatrixalgebra.html#gae95e6a42f2cce467f30a55289a0f9824" title="Obtains the eigen-decomposition of a symmetric matrix.">eigenDecomposition</a>(MtM, lambda, Q, use_gsl?<a class="code" href="group__qvmatrixalgebra.html#ggae21e0895ad3ca32f8fdc58b4bf22bec8a26b778a18b5ed84f1c9dea5d8419981e">GSL_EIGENSYMM</a>:<a class="code" href="group__qvmatrixalgebra.html#ggae21e0895ad3ca32f8fdc58b4bf22bec8a7f4f96ab3bd135f5842b0d25bb5d86a1">LAPACK_DSYEV</a>);
<a name="l00309"></a>00309 
<a name="l00310"></a>00310                 <span class="comment">// Square of the elements for vector Lambda.</span>
<a name="l00311"></a>00311                 <span class="keywordtype">double</span>  *dataLambda = lambda.data();
<a name="l00312"></a>00312                 <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i=0; i&lt;lambda.size(); i++)
<a name="l00313"></a>00313                     dataLambda[i] = sqrt(fabs(dataLambda[i]));
<a name="l00314"></a>00314 
<a name="l00315"></a>00315                 <span class="comment">// Este código obtiene la matriz &#39;Q*diag(lambda)&#39;, en la variable &#39;Q&#39;.</span>
<a name="l00316"></a>00316                 <span class="keywordtype">double</span>  *dataQ = Q.<a class="code" href="classQVMatrix.html#a0c5b1e685d14d230c55cfcfb058e372a" title="Gets a reference of the data buffer of the matrix for read and write accesses.">getWriteData</a>();
<a name="l00317"></a>00317                 <span class="keyword">const</span> <span class="keywordtype">int</span> cols = Q.<a class="code" href="classQVMatrix.html#a420bba03aeccbd18161418049a025f66" title="Get width of the matrix.">getCols</a>(), rows = Q.<a class="code" href="classQVMatrix.html#a4108aa685baecab8a9822dcc04e98b7f" title="Get height of the matrix.">getRows</a>();
<a name="l00318"></a>00318                 <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i = 0, idx = 0; i &lt; rows; i++)
<a name="l00319"></a>00319                     <span class="keywordflow">for</span>(<span class="keywordtype">int</span> j = 0; j &lt; cols; j++, idx++)
<a name="l00320"></a>00320                         dataQ[idx] *= dataLambda[j];
<a name="l00321"></a>00321 
<a name="l00322"></a>00322                 <span class="keywordflow">return</span> Q;
<a name="l00323"></a>00323                 }
<a name="l00324"></a>00324                 <span class="keywordflow">break</span>;
<a name="l00325"></a>00325             }
<a name="l00326"></a>00326         }
<a name="l00327"></a>00327     <span class="keywordflow">return</span> get8PointsCoefficientMatrix(matchingsList, normalize).<a class="code" href="classQVMatrix.html#a2f87710c9d8ae4b07b03605daea3782e" title="Change the order of the indexes in the matrix.">transpose</a>();
<a name="l00328"></a>00328     }
<a name="l00329"></a>00329 
<a name="l00330"></a><a class="code" href="group__qvsfm.html#ga20979b415197687de5b70baf9dfa2417">00330</a> <a class="code" href="classQVDirectedGraph.html" title="Directed graph class.">QVDirectedGraph&lt; QVector&lt;QPointFMatching&gt;</a> &gt; <a class="code" href="group__qvsfm.html#ga20979b415197687de5b70baf9dfa2417" title="Obtains the list of point correspondences detected between each view-pair in a reconstruction...">getPointMatchingsListsVec</a>(<span class="keyword">const</span> QList&lt;QHash&lt;int, QPointF&gt; &gt; pointProjections, <span class="keyword">const</span> <span class="keywordtype">int</span> numCams, <span class="keyword">const</span> <span class="keywordtype">int</span> minPointCorrespondences)
<a name="l00331"></a>00331     {
<a name="l00332"></a>00332     QTime time;
<a name="l00333"></a>00333     QHash&lt;int, QPointF&gt; projections;
<a name="l00334"></a>00334 
<a name="l00335"></a>00335     <span class="comment">// Transversing the structure &#39;pointProjections&#39; is not computationally expensive.</span>
<a name="l00336"></a>00336     <span class="comment">// Precompute number of correspondences between each view pair.</span>
<a name="l00337"></a>00337     <span class="comment">// time.start();</span>
<a name="l00338"></a>00338     QVector&lt; int &gt; count(numCams*numCams, 0);
<a name="l00339"></a>00339     <span class="keywordflow">foreach</span>(projections, pointProjections)                      <span class="comment">// 3D points</span>
<a name="l00340"></a>00340         {
<a name="l00341"></a>00341         <span class="keyword">const</span> QVector&lt;int&gt; indexes = projections.keys().toVector();
<a name="l00342"></a>00342         <span class="keywordflow">foreach</span>(<span class="keywordtype">int</span> i, indexes)
<a name="l00343"></a>00343             <span class="keywordflow">foreach</span>(<span class="keywordtype">int</span> j, indexes)
<a name="l00344"></a>00344                 count[i * numCams + j]++;
<a name="l00345"></a>00345         }
<a name="l00346"></a>00346     <span class="comment">// int time1 = time.elapsed();</span>
<a name="l00347"></a>00347 
<a name="l00348"></a>00348     <span class="comment">// time.start();</span>
<a name="l00349"></a>00349     QVector&lt; QVector&lt;QPointFMatching&gt; &gt; vX(numCams*numCams);
<a name="l00350"></a>00350     <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i = 0; i &lt; numCams; i++)
<a name="l00351"></a>00351         <span class="keywordflow">for</span>(<span class="keywordtype">int</span> j = 0; j &lt; numCams; j++)
<a name="l00352"></a>00352             {
<a name="l00353"></a>00353             <span class="keyword">const</span> <span class="keywordtype">int</span> c = count[i*numCams+j];
<a name="l00354"></a>00354             <span class="keywordflow">if</span> (c &gt; 0)
<a name="l00355"></a>00355                 vX[i*numCams+j].reserve(c);
<a name="l00356"></a>00356             }
<a name="l00357"></a>00357     <span class="comment">// int time2 = time.elapsed();</span>
<a name="l00358"></a>00358 
<a name="l00359"></a>00359     <span class="comment">// Gets the lists of point matchings.</span>
<a name="l00360"></a>00360     <span class="comment">// Main performance bottleneck of function &#39;getPointMatchingsLists&#39;.</span>
<a name="l00361"></a>00361     time.start();
<a name="l00362"></a>00362     <span class="keywordflow">foreach</span>(projections, pointProjections)
<a name="l00363"></a>00363         {
<a name="l00364"></a>00364         <span class="keyword">const</span> QVector&lt;int&gt; indexes = projections.keys().toVector();
<a name="l00365"></a>00365 
<a name="l00366"></a>00366         <span class="comment">// The most computationally costly part in this process is copying QPoint objects</span>
<a name="l00367"></a>00367         <span class="comment">// to QPointFMatching data structures, and storing them at their corresponding lists.</span>
<a name="l00368"></a>00368         <span class="keywordflow">foreach</span>(<span class="keywordtype">int</span> i, indexes)
<a name="l00369"></a>00369             <span class="keywordflow">foreach</span>(<span class="keywordtype">int</span> j, indexes)
<a name="l00370"></a>00370                 <span class="keywordflow">if</span> ( (i &lt; j) and (count[i*numCams+j] &gt;= minPointCorrespondences) )
<a name="l00371"></a>00371                     {
<a name="l00372"></a>00372                     QVector&lt;QPointFMatching&gt; &amp;vector = vX[i * numCams + j];
<a name="l00373"></a>00373                     vector &lt;&lt; <a class="code" href="group__qvmath.html#ga818fe0c689af956cd14431148c879718">QPointFMatching</a>(projections[i], projections[j]);
<a name="l00374"></a>00374                     <span class="comment">//const int size = vector.size();</span>
<a name="l00375"></a>00375                     <span class="comment">//vector.resize(size +1);</span>
<a name="l00376"></a>00376                     <span class="comment">//QPointFMatching &amp;p = vector[size];</span>
<a name="l00377"></a>00377                     <span class="comment">//p.first = projections[i];</span>
<a name="l00378"></a>00378                     <span class="comment">//p.second = projections[j];</span>
<a name="l00379"></a>00379                     }
<a name="l00380"></a>00380         }
<a name="l00381"></a>00381     <span class="comment">// int time3 = time.elapsed();</span>
<a name="l00382"></a>00382 
<a name="l00383"></a>00383     <span class="comment">// time.start();</span>
<a name="l00384"></a>00384     <a class="code" href="classQVDirectedGraph.html" title="Directed graph class.">QVDirectedGraph&lt; QVector&lt;QPointFMatching&gt;</a> &gt; result2;
<a name="l00385"></a>00385     <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i = 0; i &lt; numCams; i++)
<a name="l00386"></a>00386         <span class="keywordflow">for</span>(<span class="keywordtype">int</span> j = 0; j &lt; numCams; j++)
<a name="l00387"></a>00387             {
<a name="l00388"></a>00388             <span class="keyword">const</span> QVector&lt;QPointFMatching&gt; &amp; list = vX[i*numCams+j];
<a name="l00389"></a>00389             <span class="keywordflow">if</span> (list.count() &gt; 0)
<a name="l00390"></a>00390                 result2.<a class="code" href="classQVDirectedGraph.html#a4195d1920e7e5f31709cfe729560210a" title="Add a link between two nodes of the graph.">insert</a>(i,j,list);
<a name="l00391"></a>00391             }
<a name="l00392"></a>00392     <span class="comment">// int time4 = time.elapsed();</span>
<a name="l00393"></a>00393 
<a name="l00394"></a>00394 
<a name="l00395"></a>00395     <span class="comment">//std::cout &lt;&lt; &quot;*** time1 = &quot; &lt;&lt; time1 &lt;&lt; std::endl;</span>
<a name="l00396"></a>00396     <span class="comment">//std::cout &lt;&lt; &quot;*** time2 = &quot; &lt;&lt; time2 &lt;&lt; std::endl;</span>
<a name="l00397"></a>00397     <span class="comment">//std::cout &lt;&lt; &quot;*** time3 = &quot; &lt;&lt; time3 &lt;&lt; std::endl;</span>
<a name="l00398"></a>00398     <span class="comment">//std::cout &lt;&lt; &quot;*** time4 = &quot; &lt;&lt; time4 &lt;&lt; std::endl;</span>
<a name="l00399"></a>00399     <span class="comment">//std::cout &lt;&lt; &quot;*** time5 = &quot; &lt;&lt; time5 &lt;&lt; std::endl;</span>
<a name="l00400"></a>00400     <span class="comment">//std::cout &lt;&lt; &quot;*** time6 = &quot; &lt;&lt; time6 &lt;&lt; std::endl;</span>
<a name="l00401"></a>00401     <span class="comment">//std::cout &lt;&lt; &quot;*** time7 = &quot; &lt;&lt; time7 &lt;&lt; std::endl;</span>
<a name="l00402"></a>00402     <span class="keywordflow">return</span> result2;
<a name="l00403"></a>00403     }
<a name="l00404"></a>00404 
<a name="l00405"></a>00405 
<a name="l00406"></a>00406 <span class="comment">// -----------------------------------------------------------------------------</span>
<a name="l00407"></a>00407 
<a name="l00408"></a>00408 <span class="comment">// Thist function obtains the Jacobian matrix, and the residual vector</span>
<a name="l00409"></a>00409 <span class="comment">// for the GEA cost error.</span>
<a name="l00410"></a>00410 <span class="comment">// This is an internal function, used by &#39;incrementalGEA&#39;.</span>
<a name="l00411"></a>00411 <span class="keywordtype">bool</span> evaluateGEAJacobianAndResidual(    <span class="comment">// Number of cameras.</span>
<a name="l00412"></a>00412                     <span class="keyword">const</span> <span class="keywordtype">int</span> numCameras,
<a name="l00413"></a>00413                     <span class="comment">// Degrees of freedom for each camera (DOF): size of each camera in the vector &#39;x&#39;.</span>
<a name="l00414"></a>00414                     <span class="keyword">const</span> <span class="keywordtype">int</span> DOF,
<a name="l00415"></a>00415                     <span class="comment">// Graph containing the reduced matrix, for each view link.</span>
<a name="l00416"></a>00416                     <span class="keyword">const</span> <a class="code" href="classQVDirectedGraph.html" title="Directed graph class.">QVDirectedGraph&lt;QVMatrix&gt;</a> &amp;reducedMatricesGraph,
<a name="l00417"></a>00417                     <span class="comment">// List of pair-views included in the cost error.</span>
<a name="l00418"></a>00418                     <span class="keyword">const</span> QList&lt;QVGraphLink&gt; &amp;links,
<a name="l00419"></a>00419                     <span class="comment">// Vector containing the components of the cameras.</span>
<a name="l00420"></a>00420                     <span class="keyword">const</span> <a class="code" href="classQVVector.html" title="Implementation of numerical vectors.">QVVector</a> &amp;x,
<a name="l00421"></a>00421                     <span class="comment">// Overriden on output with the Hessian matrix.</span>
<a name="l00422"></a>00422                     <a class="code" href="classQVSparseBlockMatrix.html" title="Implementation of sparse block matrices.">QVSparseBlockMatrix</a> &amp;estimatedJ,
<a name="l00423"></a>00423                     <span class="comment">// Overriden on output with the objective vector.</span>
<a name="l00424"></a>00424                     <a class="code" href="classQVVector.html" title="Implementation of numerical vectors.">QVVector</a> &amp;residuals
<a name="l00425"></a>00425                     )
<a name="l00426"></a>00426     {
<a name="l00427"></a>00427     <span class="keyword">const</span> <span class="keywordtype">int</span> numLinks = links.count();
<a name="l00428"></a>00428     estimatedJ = <a class="code" href="classQVSparseBlockMatrix.html" title="Implementation of sparse block matrices.">QVSparseBlockMatrix</a>(numLinks, numCameras, 9, DOF);
<a name="l00429"></a>00429     residuals = <a class="code" href="classQVVector.html" title="Implementation of numerical vectors.">QVVector</a>(numLinks*9, 0.0);
<a name="l00430"></a>00430 
<a name="l00431"></a>00431     <span class="comment">// Pointers to the data of the vectors &#39;x&#39; and &#39;objectives&#39;.</span>
<a name="l00432"></a>00432     <span class="keyword">const</span> <span class="keywordtype">double</span>        *xData = x.constData();
<a name="l00433"></a>00433 
<a name="l00434"></a>00434 <span class="preprocessor">    #ifdef DEBUG</span>
<a name="l00435"></a>00435 <span class="preprocessor"></span>    <span class="keywordflow">if</span> (x.<a class="code" href="classQVVector.html#a29235517605f49f2327df62d5e0295df" title="Checks whether the vector contains a NaN value or not.">containsNaN</a>())
<a name="l00436"></a>00436         {
<a name="l00437"></a>00437         std::cout &lt;&lt; <span class="stringliteral">&quot;[evaluateGEAJacobianAndResidual] Error: NaN value found in reconstruction&quot;</span> &lt;&lt; std::endl;
<a name="l00438"></a>00438         <span class="keywordflow">return</span> <span class="keyword">false</span>;
<a name="l00439"></a>00439         }
<a name="l00440"></a>00440 <span class="preprocessor">    #endif</span>
<a name="l00441"></a>00441 <span class="preprocessor"></span>
<a name="l00442"></a>00442     <span class="comment">// Loop 1. Evaluate the block-jacobians for each pair of views</span>
<a name="l00443"></a>00443     <span class="comment">// related by a reduced measurement matrix M.</span>
<a name="l00444"></a>00444     <span class="keywordtype">int</span> linkIndex = 0;
<a name="l00445"></a>00445     <span class="keywordflow">foreach</span>(<a class="code" href="qvdirectedgraph_8h.html#a75d6b5c47d6dbccd956af9be24692e46" title="Link between two nodes in a graph.">QVGraphLink</a> link, links)
<a name="l00446"></a>00446         {
<a name="l00447"></a>00447         <span class="comment">// Indexes for the pair of views in the adjacency graph.</span>
<a name="l00448"></a>00448         <span class="keyword">const</span> <span class="keywordtype">int</span>       xIndex = link.x(),      <span class="comment">// Index &#39;x&#39; for the first camera.</span>
<a name="l00449"></a>00449                 yIndex = link.y();      <span class="comment">// Index &#39;y&#39; for the second camera.</span>
<a name="l00450"></a>00450 
<a name="l00451"></a>00451         <span class="comment">// Sanity check.</span>
<a name="l00452"></a>00452         <span class="keywordflow">if</span> (xIndex &gt; yIndex)
<a name="l00453"></a>00453             {
<a name="l00454"></a>00454             std::cout &lt;&lt; <span class="stringliteral">&quot;[evaluateGEAHessianAndObjectiveVector] Error: provided invalid order for indexes &#39;x&#39; and &#39;y&#39;.&quot;</span> &lt;&lt; std::endl;
<a name="l00455"></a>00455             exit(0);
<a name="l00456"></a>00456             }
<a name="l00457"></a>00457 
<a name="l00458"></a>00458         <span class="keywordflow">if</span> (xIndex &gt;= numCameras)
<a name="l00459"></a>00459             {
<a name="l00460"></a>00460             std::cout &lt;&lt; <span class="stringliteral">&quot;[evaluateGEAHessianAndObjectiveVector] Error: malformed coefficient matrices graph: index &#39;x&#39; out of bounds.&quot;</span> &lt;&lt; std::endl;
<a name="l00461"></a>00461             exit(0);
<a name="l00462"></a>00462             }
<a name="l00463"></a>00463 
<a name="l00464"></a>00464         <span class="keywordflow">if</span> (yIndex &gt;= numCameras)
<a name="l00465"></a>00465             {
<a name="l00466"></a>00466             std::cout &lt;&lt; <span class="stringliteral">&quot;[evaluateGEAHessianAndObjectiveVector] Error: malformed coefficient matrices graph: index &#39;y&#39; out of bounds.&quot;</span> &lt;&lt; std::endl;
<a name="l00467"></a>00467             exit(0);
<a name="l00468"></a>00468             }
<a name="l00469"></a>00469 
<a name="l00470"></a>00470         <span class="comment">// jacRows      Rows for the matrix [D1|D2|v] explained below.</span>
<a name="l00471"></a>00471         <span class="comment">// jacCols      Cols for the matrix [D1|D2|v] explained below.</span>
<a name="l00472"></a>00472         <span class="keyword">const</span> <span class="keywordtype">int</span> jacRows = 9, jacCols = DOF*2+1;
<a name="l00473"></a>00473 
<a name="l00474"></a>00474         <span class="comment">// Evaluate partial jacobians (D1 and D2) and partial objective function vector (v).</span>
<a name="l00475"></a>00475         <span class="comment">// These elements are such:</span>
<a name="l00476"></a>00476         <span class="comment">//      [J1|J2|e] = M * [D1|D2|v]</span>
<a name="l00477"></a>00477         <span class="keywordtype">double</span>  D1D2v[jacRows*jacCols];
<a name="l00478"></a>00478 
<a name="l00479"></a>00479         <span class="comment">// Use a different function, depending on the parametrization used for the rotations.</span>
<a name="l00480"></a>00480 <span class="preprocessor">        #ifdef SO3_PARAMETRIZATION</span>
<a name="l00481"></a>00481 <span class="preprocessor"></span>            so3EssentialEvaluation(xData + DOF*xIndex, xData + DOF*yIndex, D1D2v, 1e-32);
<a name="l00482"></a>00482 <span class="preprocessor">        #else</span>
<a name="l00483"></a>00483 <span class="preprocessor"></span>            quaternionEssentialEvaluation(xData + DOF*xIndex, xData + DOF*yIndex, D1D2v);
<a name="l00484"></a>00484 <span class="preprocessor">        #endif</span>
<a name="l00485"></a>00485 <span class="preprocessor"></span>
<a name="l00486"></a>00486         <span class="comment">// Get reduced matrix M for the pair of views &#39;x,y&#39;</span>
<a name="l00487"></a>00487         <span class="comment">//const QVMatrix &amp;reducedM = reducedMatricesGraph[index];</span>
<a name="l00488"></a>00488         <span class="keyword">const</span> <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> &amp;reducedM = reducedMatricesGraph.<a class="code" href="classQVDirectedGraph.html#ab7764dfab0839dcf34778b466dbe2f3e" title="Retrieve element for a link between two nodes.">value</a>(xIndex, yIndex);
<a name="l00489"></a>00489         <span class="keyword">const</span> <span class="keywordtype">int</span> reducedMCols = reducedM.<a class="code" href="classQVMatrix.html#a420bba03aeccbd18161418049a025f66" title="Get width of the matrix.">getCols</a>();
<a name="l00490"></a>00490 
<a name="l00491"></a>00491 <span class="preprocessor">        #ifdef DEBUG</span>
<a name="l00492"></a>00492 <span class="preprocessor"></span>        <span class="keywordflow">if</span> (reducedM.<a class="code" href="classQVMatrix.html#a43455dfaf2e8fa71242ea1fdc8beb102" title="Checks whether the matrix contains a NaN value or not.">containsNaN</a>())
<a name="l00493"></a>00493             {
<a name="l00494"></a>00494             std::cout &lt;&lt; <span class="stringliteral">&quot;[evaluateGEAJacobianAndResidual] Error: NaN value found in reduced matrix for views &quot;</span> &lt;&lt; xIndex &lt;&lt; <span class="stringliteral">&quot;, &quot;</span> &lt;&lt; yIndex &lt;&lt; std::endl;
<a name="l00495"></a>00495             <span class="keywordflow">return</span> <span class="keyword">false</span>;
<a name="l00496"></a>00496             }
<a name="l00497"></a>00497 <span class="preprocessor">        #endif</span>
<a name="l00498"></a>00498 <span class="preprocessor"></span>
<a name="l00499"></a>00499         <span class="comment">// Get the matrix of jacobians and evaluation vector with the following operation:</span>
<a name="l00500"></a>00500         <span class="comment">//      [J1|J2|e] = M x [D1|D2|v]</span>
<a name="l00501"></a>00501         <span class="comment">//</span>
<a name="l00502"></a>00502         <span class="keywordtype">double</span>  J1J2e[jacRows*jacCols];         <span class="comment">// [J1|J2|e]</span>
<a name="l00503"></a>00503         cblas_dgemm(CblasRowMajor, CblasTrans, CblasNoTrans, reducedMCols, jacCols, jacRows, 1.0, reducedM.<a class="code" href="classQVMatrix.html#a50de15cc657c8a1d9fcc749161192a64" title="Gets a read-only reference to the data buffer of the matrix.">getReadData</a>(), reducedMCols, D1D2v, jacCols, 0.0, J1J2e, jacCols);
<a name="l00504"></a>00504         <span class="keyword">const</span> <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> mJ1J2e(jacRows, jacCols, J1J2e);
<a name="l00505"></a>00505 
<a name="l00506"></a>00506 <span class="preprocessor">        #ifdef DEBUG</span>
<a name="l00507"></a>00507 <span class="preprocessor"></span>        <span class="keywordflow">if</span> (mJ1J2e.containsNaN())
<a name="l00508"></a>00508             {
<a name="l00509"></a>00509             std::cout &lt;&lt; <span class="stringliteral">&quot;[evaluateGEAJacobianAndResidual] Error: NaN value found in block jacobian for views &quot;</span> &lt;&lt; xIndex &lt;&lt; <span class="stringliteral">&quot;, &quot;</span> &lt;&lt; yIndex &lt;&lt; std::endl;
<a name="l00510"></a>00510             <span class="keywordflow">return</span> <span class="keyword">false</span>;
<a name="l00511"></a>00511             }
<a name="l00512"></a>00512 <span class="preprocessor">        #endif</span>
<a name="l00513"></a>00513 <span class="preprocessor"></span>
<a name="l00514"></a>00514         <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> mJ1(9, DOF, 0.0), mJ2(9, DOF, 0.0);
<a name="l00515"></a>00515         <a class="code" href="classQVVector.html" title="Implementation of numerical vectors.">QVVector</a> r(9, 0.0);
<a name="l00516"></a>00516         <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i = 0; i &lt; reducedMCols; i++)
<a name="l00517"></a>00517             {
<a name="l00518"></a>00518             r[i] = mJ1J2e(i, DOF*2);
<a name="l00519"></a>00519             <span class="keywordflow">for</span>(<span class="keywordtype">int</span> j = 0; j &lt; DOF; j++)
<a name="l00520"></a>00520                 {
<a name="l00521"></a>00521                 mJ1(i,j) = mJ1J2e(i,j);
<a name="l00522"></a>00522                 mJ2(i,j) = mJ1J2e(i,DOF+j);
<a name="l00523"></a>00523                 }
<a name="l00524"></a>00524             }
<a name="l00525"></a>00525 
<a name="l00526"></a>00526         estimatedJ.<a class="code" href="classQVSparseBlockMatrix.html#a0cb729d3480d161417812d5ddbc2b378" title="Set a data block.">setBlock</a>(linkIndex, xIndex, mJ1);                            <span class="comment">// Hessian block &#39;x,y&#39; = J1^T*J2</span>
<a name="l00527"></a>00527         estimatedJ.<a class="code" href="classQVSparseBlockMatrix.html#a0cb729d3480d161417812d5ddbc2b378" title="Set a data block.">setBlock</a>(linkIndex, yIndex, mJ2);                            <span class="comment">// Hessian block &#39;x,y&#39; = J1^T*J2</span>
<a name="l00528"></a>00528         <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i = 0; i &lt; 9; i++)
<a name="l00529"></a>00529             residuals[linkIndex*9+i] = mJ1J2e(i, jacCols-1);
<a name="l00530"></a>00530 
<a name="l00531"></a>00531         linkIndex++;
<a name="l00532"></a>00532         }
<a name="l00533"></a>00533 
<a name="l00534"></a>00534     <span class="keywordflow">return</span> <span class="keyword">true</span>;
<a name="l00535"></a>00535     }
<a name="l00536"></a>00536 
<a name="l00537"></a>00537 <span class="comment">// Thist function obtains the Hessian matrix, and the objective vector</span>
<a name="l00538"></a>00538 <span class="comment">// for the second level system in each Levenger-Marquardt iteration</span>
<a name="l00539"></a>00539 <span class="comment">// of the GEA optimization.</span>
<a name="l00540"></a>00540 <span class="comment">// This is an internal function, used by &#39;globalEpipolarAdjustment&#39;.</span>
<a name="l00541"></a>00541 <span class="keywordtype">bool</span> evaluateGEAHessianAndObjectiveVector(
<a name="l00542"></a>00542                                                 <span class="comment">// Number of cameras.</span>
<a name="l00543"></a>00543                         <span class="keyword">const</span> <span class="keywordtype">int</span> numCameras,
<a name="l00544"></a>00544                         <span class="comment">// Degrees of freedom for each camera (DOF): size of each camera in the vector &#39;x&#39;.</span>
<a name="l00545"></a>00545                         <span class="keyword">const</span> <span class="keywordtype">int</span> DOF,
<a name="l00546"></a>00546                         <span class="comment">// Graph containing the reduced matrix, for each view link.</span>
<a name="l00547"></a>00547                         <span class="keyword">const</span> <a class="code" href="classQVDirectedGraph.html" title="Directed graph class.">QVDirectedGraph&lt;QVMatrix&gt;</a> &amp;reducedMatricesGraph,
<a name="l00548"></a>00548                         <span class="comment">// A vector of booleans, one for each camera, indicating whether it is fixed or not.</span>
<a name="l00549"></a>00549                                                 <span class="keyword">const</span> QVector&lt;bool&gt; &amp;freeCamera,
<a name="l00550"></a>00550                         <span class="comment">// Vector containing the components of the cameras.</span>
<a name="l00551"></a>00551                         <span class="keyword">const</span> <a class="code" href="classQVVector.html" title="Implementation of numerical vectors.">QVVector</a> &amp;x,
<a name="l00552"></a>00552                         <span class="comment">// Overriden on output with the Hessian matrix.</span>
<a name="l00553"></a>00553                         <a class="code" href="classQVSparseBlockMatrix.html" title="Implementation of sparse block matrices.">QVSparseBlockMatrix</a> &amp;estimatedH,
<a name="l00554"></a>00554                         <span class="comment">// Overriden on output with the objective vector.</span>
<a name="l00555"></a>00555                         <a class="code" href="classQVVector.html" title="Implementation of numerical vectors.">QVVector</a> &amp;objectives
<a name="l00556"></a>00556                         )
<a name="l00557"></a>00557     {
<a name="l00558"></a>00558         Q_ASSERT_X(not x.<a class="code" href="classQVVector.html#a29235517605f49f2327df62d5e0295df" title="Checks whether the vector contains a NaN value or not.">containsNaN</a>(),                                 <span class="stringliteral">&quot;[evaluateGEAHessianAndObjectiveVector2]&quot;</span>, <span class="stringliteral">&quot;NaN value found in state vector&quot;</span>);
<a name="l00559"></a>00559 
<a name="l00560"></a>00560     <span class="comment">// Clear objective vector and Hessian matrix.</span>
<a name="l00561"></a>00561     objectives = <a class="code" href="classQVVector.html" title="Implementation of numerical vectors.">QVVector</a>(DOF*numCameras, 0.0);
<a name="l00562"></a>00562     estimatedH = <a class="code" href="classQVSparseBlockMatrix.html" title="Implementation of sparse block matrices.">QVSparseBlockMatrix</a>(numCameras, numCameras, DOF, DOF);
<a name="l00563"></a>00563 
<a name="l00564"></a>00564     <span class="comment">// Block elements at the diagonal of the sparse matrix &#39;estimatedH&#39; will not be updated in the loop 1.</span>
<a name="l00565"></a>00565     <span class="comment">// Instead, the values of these blocks will be accumulated on the vector &#39;diags&#39;.</span>
<a name="l00566"></a>00566     <span class="comment">// They will be translated to the Hessian matrix diagonal block elements in a final step,</span>
<a name="l00567"></a>00567     <span class="comment">// posterior to the loop 1.</span>
<a name="l00568"></a>00568     <span class="keywordtype">double</span> diags[numCameras*DOF*DOF];
<a name="l00569"></a>00569     <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i = 0; i &lt; numCameras*DOF*DOF; i++)
<a name="l00570"></a>00570         diags[i] = 0.0;
<a name="l00571"></a>00571 
<a name="l00572"></a>00572     <span class="comment">// Pointers to the data of the vectors &#39;x&#39; and &#39;objectives&#39;.</span>
<a name="l00573"></a>00573     <span class="keyword">const</span> <span class="keywordtype">double</span>        *xData = x.constData();
<a name="l00574"></a>00574     <span class="keywordtype">double</span>                      *objectivesData = objectives.data();
<a name="l00575"></a>00575 
<a name="l00576"></a>00576 <span class="preprocessor">    #ifdef DEBUG // -----------------------------------------------------------------</span>
<a name="l00577"></a>00577 <span class="preprocessor"></span>    <span class="keywordtype">bool</span> errorFound = <span class="keyword">false</span>;
<a name="l00578"></a>00578         <span class="keywordtype">int</span> jacobianEvaluations = 0;
<a name="l00579"></a>00579 <span class="preprocessor">    #endif // ------------------------ DEBUG -----------------------------------------</span>
<a name="l00580"></a>00580 <span class="preprocessor"></span>
<a name="l00581"></a>00581     <span class="comment">// Loop 1. Evaluate the block-jacobians for each pair of views</span>
<a name="l00582"></a>00582     <span class="comment">// related by a reduced measurement matrix M.</span>
<a name="l00583"></a>00583         QMapIterator&lt;QVGraphLink, QVMatrix&gt; iterator(reducedMatricesGraph);
<a name="l00584"></a>00584         <span class="keywordflow">while</span> (iterator.hasNext())
<a name="l00585"></a>00585                 {
<a name="l00586"></a>00586                 iterator.next();
<a name="l00587"></a>00587 
<a name="l00588"></a>00588         <span class="comment">// Indexes for the pair of views in the adjacency graph.</span>
<a name="l00589"></a>00589         <span class="keyword">const</span> <span class="keywordtype">int</span>       xIndex = iterator.key().x(),    <span class="comment">// Index &#39;x&#39; for the first camera.</span>
<a name="l00590"></a>00590                         yIndex = iterator.key().y();    <span class="comment">// Index &#39;y&#39; for the second camera.</span>
<a name="l00591"></a>00591 
<a name="l00592"></a>00592                 <span class="keywordflow">if</span> (not freeCamera[xIndex] and not freeCamera[yIndex])
<a name="l00593"></a>00593                         <span class="keywordflow">continue</span>;
<a name="l00594"></a>00594 
<a name="l00595"></a>00595         <span class="comment">// Sanity check.</span>
<a name="l00596"></a>00596         <span class="keywordflow">if</span> (xIndex &gt; yIndex)
<a name="l00597"></a>00597             {
<a name="l00598"></a>00598             std::cout &lt;&lt; <span class="stringliteral">&quot;[evaluateGEAHessianAndObjectiveVector] Error: provided invalid order for indexes &#39;x&#39; and &#39;y&#39;.&quot;</span> &lt;&lt; std::endl;
<a name="l00599"></a>00599             exit(0);
<a name="l00600"></a>00600             }
<a name="l00601"></a>00601 
<a name="l00602"></a>00602         <span class="keywordflow">if</span> (xIndex &gt;= numCameras)
<a name="l00603"></a>00603             {
<a name="l00604"></a>00604             std::cout &lt;&lt; <span class="stringliteral">&quot;[evaluateGEAHessianAndObjectiveVector] Error: malformed coefficient matrices graph: index &#39;x&#39; out of bounds.&quot;</span> &lt;&lt; std::endl;
<a name="l00605"></a>00605             exit(0);
<a name="l00606"></a>00606             }
<a name="l00607"></a>00607 
<a name="l00608"></a>00608         <span class="keywordflow">if</span> (yIndex &gt;= numCameras)
<a name="l00609"></a>00609             {
<a name="l00610"></a>00610             std::cout &lt;&lt; <span class="stringliteral">&quot;[evaluateGEAHessianAndObjectiveVector] Error: malformed coefficient matrices graph: index &#39;y&#39; out of bounds.&quot;</span> &lt;&lt; std::endl;
<a name="l00611"></a>00611             exit(0);
<a name="l00612"></a>00612             }
<a name="l00613"></a>00613 
<a name="l00614"></a>00614         <span class="comment">// Main data elements:</span>
<a name="l00615"></a>00615         <span class="comment">//      M       Reduced coefficient matrix.</span>
<a name="l00616"></a>00616         <span class="comment">//      e       Vectorization of essential matrix E.</span>
<a name="l00617"></a>00617         <span class="comment">//      J1      Jacobian of vector e, for the components of the first camera.</span>
<a name="l00618"></a>00618         <span class="comment">//      J2      Jacobian of vector e, for the components of the second camera.</span>
<a name="l00619"></a>00619         <span class="comment">//</span>
<a name="l00620"></a>00620         <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> J1J2(DOF,DOF);
<a name="l00621"></a>00621         <span class="keywordtype">double</span> temp1[DOF], temp2[DOF];
<a name="l00622"></a>00622         <span class="keywordtype">double</span> J1J1Data[DOF*DOF], J2J2Data[DOF*DOF];
<a name="l00623"></a>00623 
<a name="l00624"></a>00624         <span class="comment">// jacRows      Rows for the matrix [D1|D2|v] explained below.</span>
<a name="l00625"></a>00625         <span class="comment">// jacCols      Cols for the matrix [D1|D2|v] explained below.</span>
<a name="l00626"></a>00626         <span class="keyword">const</span> <span class="keywordtype">int</span>       jacRows = 9,
<a name="l00627"></a>00627                             jacCols = DOF*2+1;
<a name="l00628"></a>00628 
<a name="l00629"></a>00629         <span class="comment">// Evaluate partial jacobians (D1 and D2) and partial objective function vector (v).</span>
<a name="l00630"></a>00630         <span class="comment">// These elements are such:</span>
<a name="l00631"></a>00631         <span class="comment">//      J1 = M * D1</span>
<a name="l00632"></a>00632         <span class="comment">//      J2 = M * D2</span>
<a name="l00633"></a>00633         <span class="comment">//      e = M * v</span>
<a name="l00634"></a>00634         <span class="comment">//</span>
<a name="l00635"></a>00635         <span class="keywordtype">double</span>  D1D2v[jacRows*jacCols];
<a name="l00636"></a>00636 
<a name="l00637"></a>00637         <span class="comment">// Use a different function, depending on the parametrization used for the rotations.</span>
<a name="l00638"></a>00638 <span class="preprocessor">        #ifdef SO3_PARAMETRIZATION</span>
<a name="l00639"></a>00639 <span class="preprocessor"></span>            so3EssentialEvaluation(xData + DOF*xIndex, xData + DOF*yIndex, D1D2v, 1e-32);
<a name="l00640"></a>00640 <span class="preprocessor">        #else</span>
<a name="l00641"></a>00641 <span class="preprocessor"></span>            quaternionEssentialEvaluation(xData + DOF*xIndex, xData + DOF*yIndex, D1D2v);
<a name="l00642"></a>00642 <span class="preprocessor">        #endif</span>
<a name="l00643"></a>00643 <span class="preprocessor"></span>
<a name="l00644"></a>00644         <span class="comment">// Get reduced matrix M for the pair of views &#39;x,y&#39;</span>
<a name="l00645"></a>00645         <span class="comment">//const QVMatrix &amp;reducedM = reducedMatricesGraph[index];</span>
<a name="l00646"></a>00646         <span class="keyword">const</span> <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> &amp;reducedM = iterator.value();
<a name="l00647"></a>00647         <span class="keyword">const</span> <span class="keywordtype">int</span> reducedMCols = reducedM.<a class="code" href="classQVMatrix.html#a420bba03aeccbd18161418049a025f66" title="Get width of the matrix.">getCols</a>();
<a name="l00648"></a>00648                 Q_ASSERT(reducedM.<a class="code" href="classQVMatrix.html#a4108aa685baecab8a9822dcc04e98b7f" title="Get height of the matrix.">getRows</a>() == 9);
<a name="l00649"></a>00649                 Q_ASSERT(reducedM.<a class="code" href="classQVMatrix.html#a420bba03aeccbd18161418049a025f66" title="Get width of the matrix.">getCols</a>() &lt;= 9);
<a name="l00650"></a>00650                 Q_ASSERT(reducedM.<a class="code" href="classQVMatrix.html#a420bba03aeccbd18161418049a025f66" title="Get width of the matrix.">getCols</a>() &gt; 0);
<a name="l00651"></a>00651 
<a name="l00652"></a>00652         <span class="comment">// Get the matrix of jacobians and evaluation vector with the following operation:</span>
<a name="l00653"></a>00653         <span class="comment">//      [J1|J2|e] = M x [D1|D2|v]</span>
<a name="l00654"></a>00654         <span class="comment">//</span>
<a name="l00655"></a>00655         <span class="keywordtype">double</span>  J1J2e[jacRows*jacCols];         <span class="comment">// [J1|J2|e]</span>
<a name="l00656"></a>00656         cblas_dgemm(CblasRowMajor, CblasTrans, CblasNoTrans, reducedMCols, jacCols, jacRows, 1.0, reducedM.<a class="code" href="classQVMatrix.html#a50de15cc657c8a1d9fcc749161192a64" title="Gets a read-only reference to the data buffer of the matrix.">getReadData</a>(), reducedMCols, D1D2v, jacCols, 0.0, J1J2e, jacCols);
<a name="l00657"></a>00657 
<a name="l00658"></a>00658                 <span class="keywordflow">if</span> (freeCamera[xIndex])
<a name="l00659"></a>00659                         {
<a name="l00660"></a>00660                 <span class="comment">// Update elements of the objective vector</span>
<a name="l00661"></a>00661                     cblas_dgemv(CblasRowMajor, CblasTrans, reducedMCols, DOF, 1.0, J1J2e, jacCols, J1J2e+jacCols-1, jacCols, 0.0, temp1, 1);    <span class="comment">// J1^T * e</span>
<a name="l00662"></a>00662                     cblas_daxpy(DOF, 1.0, temp1, 1, objectivesData + DOF*xIndex, 1);    <span class="comment">// objective element &#39;x&#39; += J1^T * e</span>
<a name="l00663"></a>00663                 <span class="comment">// Update diagonal block (xIndex, xIndex) of the Hessian matrix.</span>
<a name="l00664"></a>00664                         <span class="comment">//std::cout &lt;&lt; &quot; A2 &quot; &lt;&lt; std::endl;</span>
<a name="l00665"></a>00665                     cblas_dgemm(CblasRowMajor, CblasTrans, CblasNoTrans, DOF, DOF, reducedMCols, 1.0, J1J2e,    jacCols, J1J2e, jacCols, 0.0, J1J1Data, DOF);                   <span class="comment">// J1^T * J1</span>
<a name="l00666"></a>00666                         <span class="comment">//std::cout &lt;&lt; &quot; A3 &quot; &lt;&lt; std::endl;</span>
<a name="l00667"></a>00667                     cblas_daxpy(DOF*DOF, 1.0, J1J1Data, 1, diags + xIndex*DOF*DOF, 1);  <span class="comment">// Diagonal block &#39;x&#39; += J1^T*J1</span>
<a name="l00668"></a>00668                         }
<a name="l00669"></a>00669 
<a name="l00670"></a>00670                 <span class="keywordflow">if</span> (freeCamera[yIndex])
<a name="l00671"></a>00671                         {
<a name="l00672"></a>00672                 <span class="comment">// Update elements of the objective vector.</span>
<a name="l00673"></a>00673                     cblas_dgemv(CblasRowMajor, CblasTrans, reducedMCols, DOF, 1.0, J1J2e+DOF, jacCols, J1J2e+jacCols-1, jacCols, 0.0, temp2, 1);        <span class="comment">// J2^T * e</span>
<a name="l00674"></a>00674                     cblas_daxpy(DOF, 1.0, temp2, 1, objectivesData + DOF*yIndex, 1);    <span class="comment">// objective element &#39;y&#39; += J2^T * e</span>
<a name="l00675"></a>00675                 <span class="comment">// Update diagonal block (yIndex, yIndex) of the Hessian matrix.</span>
<a name="l00676"></a>00676                         <span class="comment">//std::cout &lt;&lt; &quot; A4 &quot; &lt;&lt; std::endl;</span>
<a name="l00677"></a>00677                     cblas_dgemm(CblasRowMajor, CblasTrans, CblasNoTrans, DOF, DOF, reducedMCols, 1.0, J1J2e+DOF,        jacCols, J1J2e+DOF,     jacCols, 0.0, J2J2Data, DOF);                   <span class="comment">// J2^T * J2</span>
<a name="l00678"></a>00678                         <span class="comment">//std::cout &lt;&lt; &quot; A5 &quot; &lt;&lt; std::endl;</span>
<a name="l00679"></a>00679                     cblas_daxpy(DOF*DOF, 1.0, J2J2Data, 1, diags + yIndex*DOF*DOF, 1);  <span class="comment">// Diagonal block &#39;y&#39; += J2^T*J2</span>
<a name="l00680"></a>00680                         }
<a name="l00681"></a>00681 
<a name="l00682"></a>00682                 <span class="keywordflow">if</span> (freeCamera[xIndex] and freeCamera[yIndex])
<a name="l00683"></a>00683                         {
<a name="l00684"></a>00684                 <span class="comment">// Update off-diagonal block (xIndex, yIndex) of the Hessian matrix.</span>
<a name="l00685"></a>00685                         <span class="comment">//std::cout &lt;&lt; &quot; A6 &quot; &lt;&lt; std::endl;</span>
<a name="l00686"></a>00686                 cblas_dgemm(CblasRowMajor, CblasTrans, CblasNoTrans, DOF, DOF, reducedMCols, 1.0, J1J2e,        jacCols, J1J2e+DOF,     jacCols, 0.0, J1J2.getWriteData(), DOF);        <span class="comment">// J1^T * J2</span>
<a name="l00687"></a>00687                         <span class="comment">//std::cout &lt;&lt; &quot; A7 &quot; &lt;&lt; std::endl;</span>
<a name="l00688"></a>00688                 estimatedH.<a class="code" href="classQVSparseBlockMatrix.html#a0cb729d3480d161417812d5ddbc2b378" title="Set a data block.">setBlock</a>(xIndex, yIndex, J1J2);                              <span class="comment">// Hessian block &#39;x,y&#39; = J1^T*J2</span>
<a name="l00689"></a>00689                         }
<a name="l00690"></a>00690 
<a name="l00691"></a>00691 <span class="preprocessor">        #ifdef DEBUG // -----------------------------------------------------------------</span>
<a name="l00692"></a>00692 <span class="preprocessor"></span>                jacobianEvaluations++;
<a name="l00693"></a>00693 
<a name="l00694"></a>00694         <span class="comment">// The following lines check for NaN values in the Jacobian matrices.</span>
<a name="l00695"></a>00695 
<a name="l00696"></a>00696         <span class="comment">// A NaN value can appear at:</span>
<a name="l00697"></a>00697         <span class="comment">//      - The input reduced matrix.</span>
<a name="l00698"></a>00698         <span class="comment">//      - The input view pose parameters.</span>
<a name="l00699"></a>00699         <span class="comment">//      - The estimated matrix &#39;D1D2v&#39;, containing the Jacobian matrices and the objective vector.</span>
<a name="l00700"></a>00700        <span class="comment">/* if (QVVector(DOF*DOF, xData + DOF*xIndex).containsNaN())</span>
<a name="l00701"></a>00701 <span class="comment">            {</span>
<a name="l00702"></a>00702 <span class="comment">            errorFound = true;</span>
<a name="l00703"></a>00703 <span class="comment">            std::cout &lt;&lt; &quot;[globalEpipolarAdjustment] Error: NaN value found in the vector for view pose &quot; &lt;&lt; xIndex &lt;&lt; std::endl;</span>
<a name="l00704"></a>00704 <span class="comment">            }</span>
<a name="l00705"></a>00705 <span class="comment"></span>
<a name="l00706"></a>00706 <span class="comment">        if (QVVector(DOF*DOF, xData + DOF*yIndex).containsNaN())</span>
<a name="l00707"></a>00707 <span class="comment">            {</span>
<a name="l00708"></a>00708 <span class="comment">            errorFound = true;</span>
<a name="l00709"></a>00709 <span class="comment">            std::cout &lt;&lt; &quot;[globalEpipolarAdjustment] Error: NaN value found in the vector for view pose &quot; &lt;&lt; yIndex &lt;&lt; std::endl;</span>
<a name="l00710"></a>00710 <span class="comment">            }*/</span>
<a name="l00711"></a>00711 
<a name="l00712"></a>00712         <span class="comment">// Do not check for NaN values in the &#39;D1D2v&#39; matrix if a NaN value was found at the view pose vectors.</span>
<a name="l00713"></a>00713         <span class="keywordflow">if</span> (<a class="code" href="classQVVector.html" title="Implementation of numerical vectors.">QVVector</a>(jacRows*jacCols, D1D2v).containsNaN() and not errorFound)
<a name="l00714"></a>00714             {
<a name="l00715"></a>00715             errorFound = <span class="keyword">true</span>;
<a name="l00716"></a>00716 
<a name="l00717"></a>00717             std::cout &lt;&lt; <span class="stringliteral">&quot;[globalEpipolarAdjustment] Error: NaN value found in Jacobian matrix for views (&quot;</span> &lt;&lt; xIndex &lt;&lt; <span class="stringliteral">&quot;, &quot;</span> &lt;&lt; yIndex &lt;&lt; <span class="stringliteral">&quot;).&quot;</span> &lt;&lt; std::endl;
<a name="l00718"></a>00718 
<a name="l00719"></a>00719             <span class="comment">// Try a simple diagnose of the reason why a NaN value was found at matrix &#39;D1D2v&#39;.</span>
<a name="l00720"></a>00720             <span class="keyword">const</span> <a class="code" href="classQVVector.html" title="Implementation of numerical vectors.">QVVector</a> view1(DOF, xData + DOF*xIndex), view2(DOF, xData + DOF*yIndex);
<a name="l00721"></a>00721 
<a name="l00722"></a>00722             <span class="keywordflow">if</span> (view1 == view2)
<a name="l00723"></a>00723                         std::cout &lt;&lt; <span class="stringliteral">&quot;[globalEpipolarAdjustment] Reason: poses for views &quot;</span> &lt;&lt; xIndex &lt;&lt; <span class="stringliteral">&quot; and &quot;</span> &lt;&lt; yIndex &lt;&lt; <span class="stringliteral">&quot; are numerically EQUAL.&quot;</span> &lt;&lt; std::endl;
<a name="l00724"></a>00724             <span class="keywordflow">else</span>        {
<a name="l00725"></a>00725                             std::cout &lt;&lt; <span class="stringliteral">&quot;[globalEpipolarAdjustment] View 1 pose vector:&quot;</span> &lt;&lt; std::endl &lt;&lt; <a class="code" href="classQVEuclideanMapping3.html" title="Class modeling rotation-translation transformations for 3D points.">QVEuclideanMapping3</a>(view1).toRotationTranslationMatrix() &lt;&lt; std::endl;
<a name="l00726"></a>00726                             std::cout &lt;&lt; <span class="stringliteral">&quot;[globalEpipolarAdjustment] View 2 pose vector:&quot;</span> &lt;&lt; std::endl &lt;&lt; <a class="code" href="classQVEuclideanMapping3.html" title="Class modeling rotation-translation transformations for 3D points.">QVEuclideanMapping3</a>(view2).toRotationTranslationMatrix() &lt;&lt; std::endl;
<a name="l00727"></a>00727                             std::cout &lt;&lt; <span class="stringliteral">&quot;[globalEpipolarAdjustment] Values for Jacobian matrix:&quot;</span> &lt;&lt; std::endl &lt;&lt; <a class="code" href="classQVVector.html" title="Implementation of numerical vectors.">QVVector</a>(jacRows*jacCols, D1D2v) &lt;&lt; std::endl;
<a name="l00728"></a>00728                             }
<a name="l00729"></a>00729             }
<a name="l00730"></a>00730 
<a name="l00731"></a>00731         <span class="keywordflow">if</span> (reducedM.<a class="code" href="classQVMatrix.html#a43455dfaf2e8fa71242ea1fdc8beb102" title="Checks whether the matrix contains a NaN value or not.">containsNaN</a>())
<a name="l00732"></a>00732             {
<a name="l00733"></a>00733             errorFound = <span class="keyword">true</span>;
<a name="l00734"></a>00734             std::cout &lt;&lt; <span class="stringliteral">&quot;[globalEpipolarAdjustment] Error: NaN value found in reduced matrix for views (&quot;</span> &lt;&lt; xIndex &lt;&lt; <span class="stringliteral">&quot;, &quot;</span> &lt;&lt; yIndex &lt;&lt; <span class="stringliteral">&quot;).&quot;</span> &lt;&lt; std::endl;
<a name="l00735"></a>00735             }
<a name="l00736"></a>00736 
<a name="l00737"></a>00737         <span class="comment">// If an error was found, try a diagnose in some cases.</span>
<a name="l00738"></a>00738         <span class="keywordflow">if</span> (errorFound)
<a name="l00739"></a>00739             <span class="comment">// Error diagnose.</span>
<a name="l00740"></a>00740             <span class="keywordflow">break</span>;
<a name="l00741"></a>00741 <span class="preprocessor">        #endif // ------------------------ DEBUG -----------------------------------------</span>
<a name="l00742"></a>00742 <span class="preprocessor"></span>        }
<a name="l00743"></a>00743 
<a name="l00744"></a>00744     <span class="comment">// Set the diagonal elements of the Hessian matrix corresponding to free cameras, from the vector &#39;diags&#39;.</span>
<a name="l00745"></a>00745     <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i = 0; i &lt; numCameras; i++)
<a name="l00746"></a>00746         {
<a name="l00747"></a>00747         <span class="keyword">const</span> <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> diagMatrix(DOF, DOF, diags + i*DOF*DOF);
<a name="l00748"></a>00748         estimatedH.<a class="code" href="classQVSparseBlockMatrix.html#a0cb729d3480d161417812d5ddbc2b378" title="Set a data block.">setBlock</a>(i,i, diagMatrix);
<a name="l00749"></a>00749         }
<a name="l00750"></a>00750 
<a name="l00751"></a>00751 <span class="preprocessor">    #ifdef DEBUG // -----------------------------------------------------------------</span>
<a name="l00752"></a>00752 <span class="preprocessor"></span>   <span class="comment">// std::cout &lt;&lt; &quot;[globalEpipolarAdjustment] Number of Jacobian evaluations = &quot; &lt;&lt; jacobianEvaluations &lt;&lt; std::endl;</span>
<a name="l00753"></a>00753         <span class="keywordflow">if</span> (errorFound)
<a name="l00754"></a>00754                 <span class="keywordflow">return</span> <span class="keyword">false</span>;
<a name="l00755"></a>00755 <span class="preprocessor">    #endif // ------------------------ DEBUG -----------------------------------------</span>
<a name="l00756"></a>00756 <span class="preprocessor"></span>
<a name="l00757"></a>00757     <span class="keywordflow">return</span> <span class="keyword">true</span>;
<a name="l00758"></a>00758     }
<a name="l00759"></a>00759 
<a name="l00760"></a>00760 <span class="comment">// --------------------------------------------------------------------------------------</span>
<a name="l00761"></a>00761 
<a name="l00762"></a>00762 <span class="comment">// Adds the value &#39;value&#39; to the diagonal elements of an input sparse matrix.</span>
<a name="l00763"></a>00763 <span class="keywordtype">void</span> addTrace(<span class="keyword">const</span> <span class="keywordtype">double</span> value, <a class="code" href="classQVSparseBlockMatrix.html" title="Implementation of sparse block matrices.">QVSparseBlockMatrix</a> &amp;H)
<a name="l00764"></a>00764     {
<a name="l00765"></a>00765     <span class="keywordflow">for</span>(<span class="keywordtype">int</span> k = 0; k &lt; H.<a class="code" href="classQVSparseBlockMatrix.html#a202dae52dc2ec0501ceddc53ffc31aac" title="Get majorRows from a sparse block matrix.">getMajorRows</a>(); k++)
<a name="l00766"></a>00766         {
<a name="l00767"></a>00767         <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> &amp;M = H[k][k];
<a name="l00768"></a>00768         <span class="keywordflow">for</span> (<span class="keywordtype">int</span> j = 0; j &lt; M.<a class="code" href="classQVMatrix.html#a420bba03aeccbd18161418049a025f66" title="Get width of the matrix.">getCols</a>(); j++)
<a name="l00769"></a>00769             M(j,j) += value;
<a name="l00770"></a>00770         }
<a name="l00771"></a>00771     }
<a name="l00772"></a>00772 
<a name="l00773"></a>00773 <span class="comment">// --------------------------------------------------------------------------------------</span>
<a name="l00774"></a>00774 
<a name="l00775"></a><a class="code" href="group__qvsfm.html#ga729a711984f709024f3e532219efb6b8">00775</a> QList&lt;QVCameraPose&gt; <a class="code" href="group__qvsfm.html#ga729a711984f709024f3e532219efb6b8" title="Refine the camera poses using epipolar geometry constraints.">globalEpipolarAdjustment</a>(
<a name="l00776"></a>00776                                                                                 <span class="keyword">const</span> <span class="keywordtype">int</span> numIterations,
<a name="l00777"></a>00777                                                                             <span class="keyword">const</span> QList&lt;QVCameraPose&gt; &amp;initialCameraPoses,
<a name="l00778"></a>00778                                                                             <span class="keyword">const</span> <a class="code" href="classQVDirectedGraph.html" title="Directed graph class.">QVDirectedGraph&lt;QVMatrix&gt;</a> &amp;reducedMatricesGraph,
<a name="l00779"></a>00779                                                                             <span class="keyword">const</span> <span class="keywordtype">double</span> lambda,
<a name="l00780"></a>00780                                                                             <span class="keyword">const</span> <span class="keywordtype">bool</span> adaptativeLambda,
<a name="l00781"></a>00781                                                                                 <span class="keyword">const</span> <a class="code" href="group__qvmatrixalgebra.html#gaacdaea1a26a36293bfb60ca30909ba42" title="Available methods for sparse linear system solving.">TQVSparseSolve_Method</a> solveMethod,
<a name="l00782"></a>00782                                                                             <span class="keyword">const</span> <span class="keywordtype">int</span> secondLevelIterations
<a name="l00783"></a>00783                                                                             )
<a name="l00784"></a>00784         {
<a name="l00785"></a>00785         QVector&lt;bool&gt; freeCameras(initialCameraPoses.count(), <span class="keyword">true</span>);
<a name="l00786"></a>00786         freeCameras[0] = <span class="keyword">false</span>;
<a name="l00787"></a>00787 
<a name="l00788"></a>00788         <span class="keywordflow">return</span> <a class="code" href="group__qvsfm.html#ga729a711984f709024f3e532219efb6b8" title="Refine the camera poses using epipolar geometry constraints.">globalEpipolarAdjustment</a>(numIterations, initialCameraPoses, reducedMatricesGraph, freeCameras, lambda, adaptativeLambda, solveMethod, secondLevelIterations);
<a name="l00789"></a>00789         }
<a name="l00790"></a>00790 
<a name="l00791"></a>00791 <span class="keywordtype">int</span> gea_time_eval = 0, gea_time_solve = 0;
<a name="l00792"></a><a class="code" href="group__qvsfm.html#gad245d4a452046e18d3e4854398add123">00792</a> QList&lt;QVCameraPose&gt; <a class="code" href="group__qvsfm.html#ga729a711984f709024f3e532219efb6b8" title="Refine the camera poses using epipolar geometry constraints.">globalEpipolarAdjustment</a>(
<a name="l00793"></a>00793                                                 <span class="keyword">const</span> <span class="keywordtype">int</span> numIterations,
<a name="l00794"></a>00794                         <span class="keyword">const</span> QList&lt;QVCameraPose&gt; &amp;initialCameraPoses,
<a name="l00795"></a>00795                         <span class="keyword">const</span> <a class="code" href="classQVDirectedGraph.html" title="Directed graph class.">QVDirectedGraph&lt;QVMatrix&gt;</a> &amp;reducedMatricesGraph,
<a name="l00796"></a>00796                                                 <span class="keyword">const</span> QVector&lt;bool&gt; &amp;freeCameras,
<a name="l00797"></a>00797                         <span class="keyword">const</span> <span class="keywordtype">double</span> lambda,
<a name="l00798"></a>00798                         <span class="keyword">const</span> <span class="keywordtype">bool</span> adaptativeLambda,
<a name="l00799"></a>00799                                                 <span class="keyword">const</span> <a class="code" href="group__qvmatrixalgebra.html#gaacdaea1a26a36293bfb60ca30909ba42" title="Available methods for sparse linear system solving.">TQVSparseSolve_Method</a> solveMethod,
<a name="l00800"></a>00800                         <span class="keyword">const</span> <span class="keywordtype">int</span> secondLevelIterations
<a name="l00801"></a>00801                         )
<a name="l00802"></a>00802     {
<a name="l00803"></a>00803 <span class="preprocessor">        #ifdef DEBUG</span>
<a name="l00804"></a>00804 <span class="preprocessor"></span>    std::cout &lt;&lt; <span class="stringliteral">&quot;[globalEpipolarAdjustment] Number of terms in the GEA cost error function = &quot;</span> &lt;&lt; reducedMatricesGraph.count() &lt;&lt; std::endl;
<a name="l00805"></a>00805 <span class="preprocessor">        #endif // DEBUG</span>
<a name="l00806"></a>00806 <span class="preprocessor"></span>
<a name="l00807"></a>00807     <span class="keyword">const</span> <span class="keywordtype">int</span> numCameras = initialCameraPoses.count();
<a name="l00808"></a>00808 
<a name="l00809"></a>00809     <span class="comment">// Degrees of freedom for each camera (DOF): size of each camera in the vector &#39;x&#39; obtained below.</span>
<a name="l00810"></a>00810     <span class="comment">// Depending on the parametrization of the rotations in the camera poses, this value will be 3+3</span>
<a name="l00811"></a>00811     <span class="comment">// (for a so3 rotation vector) or 4+3 (for quaternion rotation parametrization):</span>
<a name="l00812"></a>00812 <span class="preprocessor">    #ifdef SO3_PARAMETRIZATION</span>
<a name="l00813"></a>00813 <span class="preprocessor"></span>        <span class="keyword">const</span> <span class="keywordtype">int</span> DOF = 6;
<a name="l00814"></a>00814 <span class="preprocessor">    #else</span>
<a name="l00815"></a>00815 <span class="preprocessor"></span>        <span class="keyword">const</span> <span class="keywordtype">int</span> DOF = 7;
<a name="l00816"></a>00816 <span class="preprocessor">    #endif</span>
<a name="l00817"></a>00817 <span class="preprocessor"></span>
<a name="l00818"></a>00818     QTime time;
<a name="l00819"></a>00819 
<a name="l00820"></a>00820     <span class="comment">// Create vector &#39;x&#39;, containing the coordinates of each camera pose (orientations and centers) in the corresponding parametrization.</span>
<a name="l00821"></a>00821     <span class="comment">// @note Esta parte con el data-set dubrovnik, y números de vistas de 88 o más, se lleva un tiempo significativamente alto.</span>
<a name="l00822"></a>00822     time.start();
<a name="l00823"></a>00823     <a class="code" href="classQVVector.html" title="Implementation of numerical vectors.">QVVector</a> x;
<a name="l00824"></a>00824     <span class="keywordflow">foreach</span>(<a class="code" href="classQVCameraPose.html" title="Class for the camera pose of a view, in a 3D reconstruction.">QVCameraPose</a> cameraPose, initialCameraPoses)
<a name="l00825"></a>00825 <span class="preprocessor">        #ifdef SO3_PARAMETRIZATION</span>
<a name="l00826"></a>00826 <span class="preprocessor"></span>            x &lt;&lt; <a class="code" href="group__qvmath.html#ga85e11a75a24eec1686a939aa690e1942" title="Obtains the element from the Lie algebra so(3) corresponding to a rotation matrix...">lnSO3</a>(cameraPose.<a class="code" href="classQVCameraPose.html#a8643ae492e7a6f66679bf0cc2fc911c7" title="Orientation of the camera pose.">getOrientation</a>()) &lt;&lt; cameraPose.<a class="code" href="classQVCameraPose.html#a57101f0fd271736866f970c1635dd7c1" title="Center of the camera pose.">getCenter</a>();
<a name="l00827"></a>00827 <span class="preprocessor">        #else</span>
<a name="l00828"></a>00828 <span class="preprocessor"></span>            x &lt;&lt; <a class="code" href="classQVVector.html" title="Implementation of numerical vectors.">QVVector</a>(cameraPose);
<a name="l00829"></a>00829 <span class="preprocessor">        #endif</span>
<a name="l00830"></a>00830 <span class="preprocessor"></span>
<a name="l00831"></a>00831     <span class="comment">// Variables to accumulate the time spend on the main stages of the function.</span>
<a name="l00832"></a>00832     gea_time_eval = 0;
<a name="l00833"></a>00833     gea_time_solve = 0;
<a name="l00834"></a>00834 
<a name="l00835"></a>00835     <span class="comment">// Main optimization loop.</span>
<a name="l00836"></a>00836     <span class="comment">// Apply for a fixed number of iterations.</span>
<a name="l00837"></a>00837     <span class="comment">// In the near future: also stop when a maximal residual error is reached.</span>
<a name="l00838"></a>00838     QVVector xInc(numCameras * DOF, 0.0);
<a name="l00839"></a>00839     <span class="keywordflow">for</span>(<span class="keywordtype">int</span> iteration = 0; iteration &lt; numIterations; iteration++)
<a name="l00840"></a>00840         {
<a name="l00841"></a>00841         <span class="comment">// Evaluate function and Hessian estimation.</span>
<a name="l00842"></a>00842         time.start();
<a name="l00843"></a>00843         <a class="code" href="classQVSparseBlockMatrix.html" title="Implementation of sparse block matrices.">QVSparseBlockMatrix</a>     H;
<a name="l00844"></a>00844         QVVector b;
<a name="l00845"></a>00845         <span class="keywordflow">if</span> ( not evaluateGEAHessianAndObjectiveVector(numCameras, DOF, reducedMatricesGraph, freeCameras, x, H, b) )
<a name="l00846"></a>00846             {
<a name="l00847"></a>00847             std::cout &lt;&lt; <span class="stringliteral">&quot;[globalEpipolarAdjustment] Stopping optimization at iteration &quot;</span> &lt;&lt; iteration &lt;&lt; std::endl;
<a name="l00848"></a>00848             <span class="keywordflow">break</span>;
<a name="l00849"></a>00849             }
<a name="l00850"></a>00850 
<a name="l00851"></a>00851         <span class="comment">// Evaluate the trace of the matrix H.</span>
<a name="l00852"></a>00852         <span class="comment">// Add a lambda value modified by the trace to the diagonal elements of the Hessian matrix</span>
<a name="l00853"></a>00853         <span class="keyword">const</span> <span class="keywordtype">double</span> factor = adaptativeLambda? lambda * H.trace() / (numCameras*DOF): lambda;
<a name="l00854"></a>00854         addTrace(factor, H);
<a name="l00855"></a>00855 
<a name="l00856"></a>00856         gea_time_eval += time.elapsed();
<a name="l00857"></a>00857 
<a name="l00858"></a>00858         <span class="comment">// Solve for &#39;xInc&#39; the linear system:</span>
<a name="l00859"></a>00859         <span class="comment">//      H xInc = b</span>
<a name="l00860"></a>00860         time.start();
<a name="l00861"></a>00861         <a class="code" href="group__qvmatrixalgebra.html#gaf7293f186086b4b233cf51046f016158" title="Solves a sparse system of linear equations, taking advantage of sparseness to accelerate...">sparseSolve</a>(H, xInc, b, <span class="keyword">true</span>, <span class="keyword">true</span>, solveMethod, <span class="keyword">true</span>, <span class="keyword">true</span>, secondLevelIterations);
<a name="l00862"></a>00862         gea_time_solve += time.elapsed();
<a name="l00863"></a>00863 
<a name="l00864"></a>00864         <span class="comment">// Update vector &#39;x&#39;</span>
<a name="l00865"></a>00865         x = x - xInc;
<a name="l00866"></a>00866 
<a name="l00867"></a>00867 <span class="preprocessor">        #ifdef DEBUG // -----------------------------------------------------------------</span>
<a name="l00868"></a>00868 <span class="preprocessor"></span>                <span class="keywordflow">if</span> (iteration == 0)
<a name="l00869"></a>00869                 std::cout &lt;&lt; <span class="stringliteral">&quot;[globalEpipolarAdjustment] Increment for diagonal of H at iteration &quot;</span> &lt;&lt; iteration &lt;&lt; <span class="stringliteral">&quot; is &quot;</span> &lt;&lt; factor &lt;&lt; std::endl;
<a name="l00870"></a>00870 
<a name="l00871"></a>00871         <span class="comment">// Check that increment vector does not contains NaN values.</span>
<a name="l00872"></a>00872         <span class="keywordflow">if</span> (xInc.containsNaN())
<a name="l00873"></a>00873             {
<a name="l00874"></a>00874             std::cout &lt;&lt; <span class="stringliteral">&quot;[globalEpipolarAdjustment] Error: NaN value found in increment vector at iteration &quot;</span> &lt;&lt; iteration &lt;&lt; <span class="stringliteral">&quot;. Stopping.&quot;</span> &lt;&lt; std::endl;
<a name="l00875"></a>00875             <span class="keywordflow">break</span>;
<a name="l00876"></a>00876             }
<a name="l00877"></a>00877 
<a name="l00878"></a>00878                 <span class="comment">// Check that Hessian rows and cols corresponding to fixed cameras contain &#39;zero&#39; valued blocks.</span>
<a name="l00879"></a>00879         <span class="keywordflow">foreach</span>(<span class="keywordtype">int</span> ib, H.keys())
<a name="l00880"></a>00880             {
<a name="l00881"></a>00881             <span class="keyword">const</span> QMap&lt;int, QVMatrix&gt; &amp;majorRow = H[ib];
<a name="l00882"></a>00882             <span class="keywordflow">foreach</span>(<span class="keywordtype">int</span> jb, majorRow.keys())
<a name="l00883"></a>00883                                 {
<a name="l00884"></a>00884                                 <span class="comment">// Permit diagonal blocks corresponding to fixed cameras to contain a diagonal matrix.</span>
<a name="l00885"></a>00885                                 <span class="keywordflow">if</span> (ib == jb)
<a name="l00886"></a>00886                                         {
<a name="l00887"></a>00887                                         <span class="keywordflow">if</span> (not majorRow[ib].isDiagonal() and not freeCameras[ib])
<a name="l00888"></a>00888                                         std::cout &lt;&lt; <span class="stringliteral">&quot;[globalEpipolarAdjustment] Error: Diagonal Hessian block &quot;</span> &lt;&lt; ib &lt;&lt; <span class="stringliteral">&quot; is not diagonal.&quot;</span> &lt;&lt; std::endl;
<a name="l00889"></a>00889                                         <span class="keywordflow">continue</span>;
<a name="l00890"></a>00890                                         }
<a name="l00891"></a>00891                                 <span class="keywordflow">if</span> ( not freeCameras[ib] and majorRow.contains(jb) )
<a name="l00892"></a>00892                                 std::cout &lt;&lt; <span class="stringliteral">&quot;[globalEpipolarAdjustment] Error: Hessian block &quot;</span> &lt;&lt; ib &lt;&lt; <span class="stringliteral">&quot;, &quot;</span> &lt;&lt; jb &lt;&lt; <span class="stringliteral">&quot; is not zero: &quot;</span> &lt;&lt; majorRow[jb] &lt;&lt; <span class="stringliteral">&quot; while camera &quot;</span> &lt;&lt; ib &lt;&lt; <span class="stringliteral">&quot; is fixed.&quot;</span> &lt;&lt; std::endl;
<a name="l00893"></a>00893                                 <span class="keywordflow">if</span> ( not freeCameras[jb] and majorRow.contains(jb) )
<a name="l00894"></a>00894                                 std::cout &lt;&lt; <span class="stringliteral">&quot;[globalEpipolarAdjustment] Error: Hessian block &quot;</span> &lt;&lt; ib &lt;&lt; <span class="stringliteral">&quot;, &quot;</span> &lt;&lt; jb &lt;&lt; <span class="stringliteral">&quot; is not zero: &quot;</span> &lt;&lt; majorRow[jb] &lt;&lt; <span class="stringliteral">&quot; while camera &quot;</span> &lt;&lt; jb &lt;&lt; <span class="stringliteral">&quot; is fixed.&quot;</span> &lt;&lt; std::endl;
<a name="l00895"></a>00895                                 }
<a name="l00896"></a>00896             }
<a name="l00897"></a>00897 
<a name="l00898"></a>00898                 <span class="comment">// Check that the size of the increment vector is correct.</span>
<a name="l00899"></a>00899                 <span class="keywordflow">if</span> (xInc.count() != numCameras * DOF)
<a name="l00900"></a>00900                         std::cout &lt;&lt; <span class="stringliteral">&quot;[globalEpipolarAdjustment] Error: size of increment vector should be &quot;</span> &lt;&lt; (numCameras * DOF) &lt;&lt; <span class="stringliteral">&quot;(&quot;</span> &lt;&lt; numCameras &lt;&lt; <span class="stringliteral">&quot; cameras, and &quot;</span> &lt;&lt; DOF &lt;&lt; <span class="stringliteral">&quot; DOF&#39;s) but is &quot;</span> &lt;&lt; xInc.count() &lt;&lt; std::endl;
<a name="l00901"></a>00901 
<a name="l00902"></a>00902                 <span class="comment">// Check that the increment obtained for parameters corresponding to fixed cameras is zero.</span>
<a name="l00903"></a>00903                 <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i = 0, idx = 0; i &lt; numCameras; i++)
<a name="l00904"></a>00904                         <span class="keywordflow">for</span>(<span class="keywordtype">int</span> j = 0; j &lt; DOF; j++, idx++)
<a name="l00905"></a>00905                                 <span class="keywordflow">if</span> (not freeCameras[i] and xInc[idx] != 0.0)
<a name="l00906"></a>00906                                         std::cout &lt;&lt; <span class="stringliteral">&quot;[globalEpipolarAdjustment] Error: increment &quot;</span> &lt;&lt; j &lt;&lt; <span class="stringliteral">&quot; for fixed camera &quot;</span> &lt;&lt; i &lt;&lt; <span class="stringliteral">&quot; is not zero.&quot;</span> &lt;&lt; std::endl;
<a name="l00907"></a>00907 
<a name="l00908"></a>00908 <span class="preprocessor">        #endif // ------------------------ DEBUG -----------------------------------------</span>
<a name="l00909"></a>00909 <span class="preprocessor"></span>        }
<a name="l00910"></a>00910 
<a name="l00911"></a>00911     <span class="comment">// Compose the list of optimized camera poses, from the vector &#39;x&#39;.</span>
<a name="l00912"></a>00912     QList&lt;QVCameraPose&gt; optimizedCameraPoses;
<a name="l00913"></a>00913     <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i = 0; i &lt; numCameras; i++)
<a name="l00914"></a>00914         #ifdef SO3_PARAMETRIZATION
<a name="l00915"></a>00915             optimizedCameraPoses &lt;&lt; <a class="code" href="classQVCameraPose.html" title="Class for the camera pose of a view, in a 3D reconstruction.">QVCameraPose</a>(<a class="code" href="classQVQuaternion.html" title="Implementation of quaternions.">QVQuaternion</a>(<a class="code" href="group__qvmath.html#gad9a79664f7af4487d5af8fe2296fc7a4" title="Obtains the rotation matrix corresponding to the element from the Lie algebra so(3)...">expSO3</a>(<a class="code" href="classQV3DPointF.html" title="3D point representation">QV3DPointF</a>(x.mid(DOF*i, 3)))), <a class="code" href="classQV3DPointF.html" title="3D point representation">QV3DPointF</a>(x.mid(DOF*i+3,3)));
<a name="l00916"></a>00916 <span class="preprocessor">        #else</span>
<a name="l00917"></a>00917 <span class="preprocessor"></span>            optimizedCameraPoses &lt;&lt; <a class="code" href="classQVCameraPose.html" title="Class for the camera pose of a view, in a 3D reconstruction.">QVCameraPose</a>(<a class="code" href="classQVQuaternion.html" title="Implementation of quaternions.">QVQuaternion</a>(x.mid(DOF*i, 4)), <a class="code" href="classQV3DPointF.html" title="3D point representation">QV3DPointF</a>(x.mid(DOF*i+4,3)) );
<a name="l00918"></a>00918 <span class="preprocessor">        #endif</span>
<a name="l00919"></a>00919 <span class="preprocessor"></span>
<a name="l00920"></a>00920     <span class="comment">// Return the list of optimized camera poses.</span>
<a name="l00921"></a>00921     <span class="keywordflow">return</span> optimizedCameraPoses;
<a name="l00922"></a>00922     }
<a name="l00923"></a>00923 
<a name="l00924"></a><a class="code" href="group__qvsfm.html#ga854751f95bc2cd345b9f5cceaea3b192">00924</a> QList&lt;QVCameraPose&gt; <a class="code" href="group__qvsfm.html#ga854751f95bc2cd345b9f5cceaea3b192" title="Incremental GEA refinement.">incrementalGEA</a>(     <span class="keyword">const</span> <span class="keywordtype">int</span> numIterations,
<a name="l00925"></a>00925                     <span class="keyword">const</span> QList&lt;QVCameraPose&gt; &amp;initialCameraPoses,
<a name="l00926"></a>00926                     <span class="keyword">const</span> <a class="code" href="classQVDirectedGraph.html" title="Directed graph class.">QVDirectedGraph&lt;QVMatrix&gt;</a> &amp;reducedMatricesGraph,
<a name="l00927"></a>00927                     <span class="keyword">const</span> <span class="keywordtype">int</span> numFreeCameras,
<a name="l00928"></a>00928                     <span class="keyword">const</span> <span class="keywordtype">double</span> lambda,
<a name="l00929"></a>00929                     <span class="keyword">const</span> <span class="keywordtype">bool</span> adaptativeLambda,
<a name="l00930"></a>00930                     <span class="keyword">const</span> <a class="code" href="group__qvmatrixalgebra.html#gaacdaea1a26a36293bfb60ca30909ba42" title="Available methods for sparse linear system solving.">TQVSparseSolve_Method</a> solveMethod,
<a name="l00931"></a>00931                     <span class="keyword">const</span> <span class="keywordtype">int</span> secondLevelIterations
<a name="l00932"></a>00932                     )
<a name="l00933"></a>00933     {
<a name="l00934"></a>00934     std::cout &lt;&lt; <span class="stringliteral">&quot;WARNING: &#39;incrementalGEA&#39; deprecated. Use function &#39;globalEpipolarAdjustment&#39; to perform incremental GEA optimization.&quot;</span> &lt;&lt; std::endl;
<a name="l00935"></a>00935     <span class="comment">// Number of cameras.</span>
<a name="l00936"></a>00936     <span class="keyword">const</span> <span class="keywordtype">int</span>   numCameras = initialCameraPoses.count(),
<a name="l00937"></a>00937             freeCameras = MIN(numFreeCameras, numCameras),
<a name="l00938"></a>00938             fixedCameras = numCameras - freeCameras,
<a name="l00939"></a>00939             firstFreeCameraIndex = fixedCameras;
<a name="l00940"></a>00940 
<a name="l00941"></a>00941     <span class="comment">// Evaluate GEA Jacobian and residual only for terms involving one or two free cameras.</span>
<a name="l00942"></a>00942     QList&lt;QPoint&gt; incrementalLinks;
<a name="l00943"></a>00943     <span class="keywordflow">foreach</span>(QPoint link, reducedMatricesGraph.keys())
<a name="l00944"></a>00944         <span class="keywordflow">if</span> (link.x() &gt;= firstFreeCameraIndex or link.y() &gt;= firstFreeCameraIndex)
<a name="l00945"></a>00945             incrementalLinks &lt;&lt; link;
<a name="l00946"></a>00946 
<a name="l00947"></a>00947     <span class="comment">// Degrees of freedom for each camera (DOF): size of each camera in the vector &#39;x&#39; obtained below.</span>
<a name="l00948"></a>00948     <span class="comment">// Depending on the parametrization of the rotations in the camera poses, this value will be 3+3</span>
<a name="l00949"></a>00949     <span class="comment">// (for a so3 rotation vector) or 4+3 (for quaternion rotation parametrization):</span>
<a name="l00950"></a>00950 <span class="preprocessor">    #ifdef SO3_PARAMETRIZATION</span>
<a name="l00951"></a>00951 <span class="preprocessor"></span>        <span class="keyword">const</span> <span class="keywordtype">int</span> DOF = 6;
<a name="l00952"></a>00952 <span class="preprocessor">    #else</span>
<a name="l00953"></a>00953 <span class="preprocessor"></span>        <span class="keyword">const</span> <span class="keywordtype">int</span> DOF = 7;
<a name="l00954"></a>00954 <span class="preprocessor">    #endif</span>
<a name="l00955"></a>00955 <span class="preprocessor"></span>
<a name="l00956"></a>00956     QTime time;
<a name="l00957"></a>00957 
<a name="l00958"></a>00958     <span class="comment">// Create vector &#39;x&#39;, containing the coordinates of each camera pose (orientations and centers) in the corresponding parametrization.</span>
<a name="l00959"></a>00959     <span class="comment">// @note Esta parte con el data-set dubrovnik, y números de vistas de 88 o más, se lleva un tiempo significativamente alto.</span>
<a name="l00960"></a>00960     time.start();
<a name="l00961"></a>00961     <a class="code" href="classQVVector.html" title="Implementation of numerical vectors.">QVVector</a> x;
<a name="l00962"></a>00962     <span class="keywordflow">foreach</span>(<a class="code" href="classQVCameraPose.html" title="Class for the camera pose of a view, in a 3D reconstruction.">QVCameraPose</a> cameraPose, initialCameraPoses)
<a name="l00963"></a>00963 <span class="preprocessor">        #ifdef SO3_PARAMETRIZATION</span>
<a name="l00964"></a>00964 <span class="preprocessor"></span>            x &lt;&lt; <a class="code" href="group__qvmath.html#ga85e11a75a24eec1686a939aa690e1942" title="Obtains the element from the Lie algebra so(3) corresponding to a rotation matrix...">lnSO3</a>(cameraPose.<a class="code" href="classQVCameraPose.html#a8643ae492e7a6f66679bf0cc2fc911c7" title="Orientation of the camera pose.">getOrientation</a>()) &lt;&lt; cameraPose.<a class="code" href="classQVCameraPose.html#a57101f0fd271736866f970c1635dd7c1" title="Center of the camera pose.">getCenter</a>();
<a name="l00965"></a>00965 <span class="preprocessor">        #else</span>
<a name="l00966"></a>00966 <span class="preprocessor"></span>            x &lt;&lt; <a class="code" href="classQVVector.html" title="Implementation of numerical vectors.">QVVector</a>(cameraPose);
<a name="l00967"></a>00967 <span class="preprocessor">        #endif</span>
<a name="l00968"></a>00968 <span class="preprocessor"></span>    <span class="comment">// const int time_data_initialization = time.elapsed();</span>
<a name="l00969"></a>00969 
<a name="l00970"></a>00970     <span class="comment">// Variables to accumulate the time spend on the main stages of the function.</span>
<a name="l00971"></a>00971     gea_time_eval = 0;
<a name="l00972"></a>00972     gea_time_solve = 0;
<a name="l00973"></a>00973 
<a name="l00974"></a>00974     <span class="comment">// Main optimization loop.</span>
<a name="l00975"></a>00975     <span class="comment">// Apply for a fixed number of iterations.</span>
<a name="l00976"></a>00976     <span class="comment">// In the near future: also stop when a maximal residual error is reached.</span>
<a name="l00977"></a>00977     <span class="comment">//QVVector xInc(numCameras * DOF, 0.0);</span>
<a name="l00978"></a>00978     <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i = 0; i &lt; numIterations; i++)
<a name="l00979"></a>00979         {
<a name="l00980"></a>00980         <span class="comment">// Evaluate Hessian and objective vector.</span>
<a name="l00981"></a>00981         time.start();
<a name="l00982"></a>00982         <a class="code" href="classQVSparseBlockMatrix.html" title="Implementation of sparse block matrices.">QVSparseBlockMatrix</a> J;
<a name="l00983"></a>00983         QVVector r;
<a name="l00984"></a>00984         evaluateGEAJacobianAndResidual(numCameras, DOF, reducedMatricesGraph, incrementalLinks, x, J, r);
<a name="l00985"></a>00985         <a class="code" href="classQVSparseBlockMatrix.html" title="Implementation of sparse block matrices.">QVSparseBlockMatrix</a> H = J.<a class="code" href="classQVSparseBlockMatrix.html#a36a11e4a75005686b522407e67136b0c" title="Dot product for sparse block matrices.">dotProduct</a>(J, <span class="keyword">true</span>, <span class="keyword">false</span>);
<a name="l00986"></a>00986         QVVector b = J.<a class="code" href="classQVSparseBlockMatrix.html#a36a11e4a75005686b522407e67136b0c" title="Dot product for sparse block matrices.">dotProduct</a>(r, <span class="keyword">true</span>);
<a name="l00987"></a>00987 
<a name="l00988"></a>00988         <span class="comment">// Eliminate columns for the Jacobian corresponding to fixed cameras.</span>
<a name="l00989"></a>00989         <a class="code" href="classQVSparseBlockMatrix.html" title="Implementation of sparse block matrices.">QVSparseBlockMatrix</a> simplifiedJ = <a class="code" href="classQVSparseBlockMatrix.html" title="Implementation of sparse block matrices.">QVSparseBlockMatrix</a>(J.<a class="code" href="classQVSparseBlockMatrix.html#a202dae52dc2ec0501ceddc53ffc31aac" title="Get majorRows from a sparse block matrix.">getMajorRows</a>(), freeCameras, J.<a class="code" href="classQVSparseBlockMatrix.html#af6637b61809a2d386d8d6ea3078a96b6" title="Get minorRows from a sparse block matrix.">getMinorRows</a>(), J.<a class="code" href="classQVSparseBlockMatrix.html#ad606f1e2fe760064d0494e7c133dbed7" title="Get minorCols from a sparse block matrix.">getMinorCols</a>());
<a name="l00990"></a>00990         <span class="keywordflow">foreach</span>(<span class="keywordtype">int</span> ib, J.keys())
<a name="l00991"></a>00991             {
<a name="l00992"></a>00992             <span class="keyword">const</span> QMap&lt;int, QVMatrix&gt; &amp;majorRow = J[ib];
<a name="l00993"></a>00993             <span class="keywordflow">foreach</span>(<span class="keywordtype">int</span> jb, majorRow.keys())
<a name="l00994"></a>00994                 <span class="keywordflow">if</span> (jb &gt;= firstFreeCameraIndex)
<a name="l00995"></a>00995                     simplifiedJ.<a class="code" href="classQVSparseBlockMatrix.html#a0cb729d3480d161417812d5ddbc2b378" title="Set a data block.">setBlock</a>(ib, jb - firstFreeCameraIndex, majorRow[jb]);
<a name="l00996"></a>00996             }
<a name="l00997"></a>00997 
<a name="l00998"></a>00998         H = simplifiedJ.<a class="code" href="classQVSparseBlockMatrix.html#a36a11e4a75005686b522407e67136b0c" title="Dot product for sparse block matrices.">dotProduct</a>(simplifiedJ, <span class="keyword">true</span>, <span class="keyword">false</span>);
<a name="l00999"></a>00999         b = simplifiedJ.<a class="code" href="classQVSparseBlockMatrix.html#a36a11e4a75005686b522407e67136b0c" title="Dot product for sparse block matrices.">dotProduct</a>(r, <span class="keyword">true</span>);
<a name="l01000"></a>01000         gea_time_eval += time.elapsed();
<a name="l01001"></a>01001 
<a name="l01002"></a>01002         <span class="comment">// When using the &#39;sparseSolve&#39; function assuming symmetric non-singular matrix,</span>
<a name="l01003"></a>01003         <span class="comment">// the lower triangular area of the coefficient matrix should be zero.</span>
<a name="l01004"></a>01004         <a class="code" href="classQVSparseBlockMatrix.html" title="Implementation of sparse block matrices.">QVSparseBlockMatrix</a> H2(H.<a class="code" href="classQVSparseBlockMatrix.html#a202dae52dc2ec0501ceddc53ffc31aac" title="Get majorRows from a sparse block matrix.">getMajorRows</a>(), H.<a class="code" href="classQVSparseBlockMatrix.html#a6cb89146c9191b915b8f493f0be6becc" title="Get majorCols from a sparse block matrix.">getMajorCols</a>(), H.<a class="code" href="classQVSparseBlockMatrix.html#af6637b61809a2d386d8d6ea3078a96b6" title="Get minorRows from a sparse block matrix.">getMinorRows</a>(), H.<a class="code" href="classQVSparseBlockMatrix.html#ad606f1e2fe760064d0494e7c133dbed7" title="Get minorCols from a sparse block matrix.">getMinorCols</a>());
<a name="l01005"></a>01005         <span class="keywordflow">foreach</span>(<span class="keywordtype">int</span> ib, H.keys())
<a name="l01006"></a>01006             {
<a name="l01007"></a>01007             QMap&lt;int, QVMatrix&gt; &amp;majorRow = H[ib];
<a name="l01008"></a>01008             <span class="keywordflow">foreach</span>(<span class="keywordtype">int</span> jb, majorRow.keys())
<a name="l01009"></a>01009                 <span class="keywordflow">if</span> (ib &lt;= jb)
<a name="l01010"></a>01010                     H2.<a class="code" href="classQVSparseBlockMatrix.html#a0cb729d3480d161417812d5ddbc2b378" title="Set a data block.">setBlock</a>(ib, jb, majorRow[jb]);
<a name="l01011"></a>01011             }
<a name="l01012"></a>01012 
<a name="l01013"></a>01013         <span class="comment">// Program fails when using MKL to solve with H instead of H2?. It shouldn&#39;t.</span>
<a name="l01014"></a>01014         <span class="comment">//H = H2;</span>
<a name="l01015"></a>01015         <span class="comment">//std::cout &lt;&lt; &quot;[incrementalGEA] Debug: ||H - H2|| = &quot; &lt;&lt; (QVMatrix(H) - QVMatrix(H2)).norm2() / ( QVMatrix(H).norm2() + QVMatrix(H2).norm2() ) &lt;&lt; std::endl;</span>
<a name="l01016"></a>01016 
<a name="l01017"></a>01017         <span class="comment">// Evaluate the trace of the matrix H.</span>
<a name="l01018"></a>01018         <span class="comment">// Add a lambda value modified by the trace to the diagonal elements of the Hessian matrix</span>
<a name="l01019"></a>01019         <span class="keyword">const</span> <span class="keywordtype">double</span> factor = adaptativeLambda? lambda * H2.trace() / (numCameras*DOF): lambda;
<a name="l01020"></a>01020 
<a name="l01021"></a>01021 <span class="preprocessor">        #ifdef DEBUG</span>
<a name="l01022"></a>01022 <span class="preprocessor"></span>        std::cout &lt;&lt; <span class="stringliteral">&quot;[globalEpipolarAdjustment] Increment for diagonal of H at iteration &quot;</span> &lt;&lt; i &lt;&lt; <span class="stringliteral">&quot; is &quot;</span> &lt;&lt; factor &lt;&lt; std::endl;
<a name="l01023"></a>01023 <span class="preprocessor">        #endif</span>
<a name="l01024"></a>01024 <span class="preprocessor"></span>
<a name="l01025"></a>01025         addTrace(factor, H2);
<a name="l01026"></a>01026 
<a name="l01027"></a>01027         time.start();
<a name="l01028"></a>01028         QVVector xIncPartial;
<a name="l01029"></a>01029         <a class="code" href="group__qvmatrixalgebra.html#gaf7293f186086b4b233cf51046f016158" title="Solves a sparse system of linear equations, taking advantage of sparseness to accelerate...">sparseSolve</a>(H2, xIncPartial, b, <span class="keyword">true</span>, <span class="keyword">true</span>, solveMethod, <span class="keyword">true</span>, <span class="keyword">true</span>, secondLevelIterations);
<a name="l01030"></a>01030 
<a name="l01031"></a>01031         QVVector xInc(fixedCameras * DOF, 0.0);
<a name="l01032"></a>01032         xInc &lt;&lt; xIncPartial;
<a name="l01033"></a>01033         gea_time_solve += time.elapsed();
<a name="l01034"></a>01034 
<a name="l01035"></a>01035         <span class="comment">// Sanity check.</span>
<a name="l01036"></a>01036         <span class="keywordflow">if</span> (xInc.<a class="code" href="classQVVector.html#a29235517605f49f2327df62d5e0295df" title="Checks whether the vector contains a NaN value or not.">containsNaN</a>())
<a name="l01037"></a>01037             {
<a name="l01038"></a>01038             std::cout &lt;&lt; <span class="stringliteral">&quot;[globalEpipolarAdjustment] Error: NaN value found in &#39;x&#39; increment at iteration &quot;</span> &lt;&lt; i &lt;&lt; <span class="stringliteral">&quot;. Stopping.&quot;</span> &lt;&lt; std::endl;
<a name="l01039"></a>01039             <span class="keywordflow">break</span>;
<a name="l01040"></a>01040             }
<a name="l01041"></a>01041 
<a name="l01042"></a>01042         <span class="comment">// Update vector &#39;x&#39;</span>
<a name="l01043"></a>01043         x = x - xInc;
<a name="l01044"></a>01044         }
<a name="l01045"></a>01045 
<a name="l01046"></a>01046 <span class="preprocessor">    #ifdef DEBUG</span>
<a name="l01047"></a>01047 <span class="preprocessor"></span>    std::cout &lt;&lt; <span class="stringliteral">&quot;[globalEpipolarAdjustment] Number of Jacobian evaluations = &quot;</span> &lt;&lt; reducedMatricesGraph.<a class="code" href="classQVDirectedGraph.html#a1f2447948be415119b3956715b8fe8b0" title="Retrieve links defined between the nodes of the graph.">getLinks</a>().count() &lt;&lt; std::endl;
<a name="l01048"></a>01048 <span class="preprocessor">    #endif</span>
<a name="l01049"></a>01049 <span class="preprocessor"></span>
<a name="l01050"></a>01050     <span class="comment">// Compose the list of optimized camera poses, from the vector &#39;x&#39;.</span>
<a name="l01051"></a>01051     QList&lt;QVCameraPose&gt; optimizedCameraPoses;
<a name="l01052"></a>01052     <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i = 0; i &lt; numCameras; i++)
<a name="l01053"></a>01053         #ifdef SO3_PARAMETRIZATION
<a name="l01054"></a>01054             optimizedCameraPoses &lt;&lt; <a class="code" href="classQVCameraPose.html" title="Class for the camera pose of a view, in a 3D reconstruction.">QVCameraPose</a>(<a class="code" href="classQVQuaternion.html" title="Implementation of quaternions.">QVQuaternion</a>(<a class="code" href="group__qvmath.html#gad9a79664f7af4487d5af8fe2296fc7a4" title="Obtains the rotation matrix corresponding to the element from the Lie algebra so(3)...">expSO3</a>(<a class="code" href="classQV3DPointF.html" title="3D point representation">QV3DPointF</a>(x.mid(DOF*i, 3)))), <a class="code" href="classQV3DPointF.html" title="3D point representation">QV3DPointF</a>(x.mid(DOF*i+3,3)));
<a name="l01055"></a>01055 <span class="preprocessor">        #else</span>
<a name="l01056"></a>01056 <span class="preprocessor"></span>            optimizedCameraPoses &lt;&lt; <a class="code" href="classQVCameraPose.html" title="Class for the camera pose of a view, in a 3D reconstruction.">QVCameraPose</a>(<a class="code" href="classQVQuaternion.html" title="Implementation of quaternions.">QVQuaternion</a>(x.mid(DOF*i, 4)), <a class="code" href="classQV3DPointF.html" title="3D point representation">QV3DPointF</a>(x.mid(DOF*i+4,3)) );
<a name="l01057"></a>01057 <span class="preprocessor">        #endif</span>
<a name="l01058"></a>01058 <span class="preprocessor"></span>
<a name="l01059"></a>01059     <span class="comment">// Return the list of optimized camera poses.</span>
<a name="l01060"></a>01060     <span class="keywordflow">return</span> optimizedCameraPoses;
<a name="l01061"></a>01061     }
<a name="l01062"></a>01062 
</pre></div></div>
</td></tr></table>

<br /><hr><br />
<center><a href="http://perception.inf.um.es/QVision">QVision framework</a>.
<a href="http://perception.inf.um.es">PARP research group</a>.
Copyright &copy; 2007, 2008, 2009, 2010, 2011.</center>
<br />
</body>
</html>
